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

Abstract

The system creates a driving plan that ensures the vehicle travels within a safety zone set according to its speed, while minimizing deviation from the target route. [Solution] The driving plan device 30 includes an initial processing unit 102 that acquires map information and a target route, an acquisition unit 104 that acquires the initial position and attitude of the vehicle, an update unit 106 that updates the future position and attitude of the vehicle at predetermined update intervals based on the initial position and attitude of the vehicle, a speed planning unit 108 that sets a series of candidate vehicle positions within a predicted target time range longer than the future update interval based on the future position and attitude of the vehicle, and determines a planned vehicle speed that, when used for driving the vehicle, will prevent obstacles from entering the safety area over the predicted target time range and allow the vehicle to travel with a small deviation from the target route, based on the map information, target route, and candidate position series, and a generation unit 110 that generates a planned speed series for each update cycle.

Description

[Technical Field]

[0001] This disclosure relates to a travel planning device, a travel planning method, and a travel planning program. [Background technology]

[0002] Patent Document 1 discloses a device that plans control inputs to ensure that obstacles do not enter a safety area, which has a length that increases with increasing speed from the front of the vehicle forward. According to this device, the vehicle's speed is reduced when an obstacle is detected within the safety area. Specifically, this device first acquires map information showing the starting point, the end point, a reference path from the starting point to the end point (for example, a line connecting the central points of the passage width), and the arrangement of obstacles in the driving area. Then, it plans a sequence of translational speed and steering angle of the vehicle so that the vehicle can reach the end point from the starting point in the shortest possible time while ensuring that the safety area and the vehicle body do not interfere with obstacles. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2023-154648 [Overview of the Initiative] [Problems that the invention aims to solve]

[0004] Incidentally, when acquiring information on the starting point, goal point, and the placement of obstacles within the driving area before driving, and then determining a target path that will allow the vehicle to reach the goal point without interfering with obstacles, a method is needed to plan control inputs that allow the vehicle to drive while minimizing deviation from the target path, while ensuring that the safe area and the vehicle body do not interfere with obstacles.

[0005] The purpose of this disclosure is to provide a driving plan device, a driving plan method, and a driving plan program that create a driving plan that allows a vehicle to travel while suppressing deviation from the target path, while ensuring that obstacles do not enter a safety area set according to the vehicle's speed. [Means for solving the problem]

[0006] To achieve the above objective, the vehicle driving plan device according to this disclosure is a vehicle driving plan device in which a safety area is set in which the length increases as the translational speed of the vehicle increases and the width approximates the width of the vehicle, extending forward from the front end of the vehicle, and comprises: an initial processing unit that acquires map information showing the arrangement of obstacles and a target path set so that the vehicle body does not interfere with the obstacles during the vehicle's driving; an acquisition unit that acquires the initial position and attitude of the vehicle; an update unit that updates the future position and attitude of the vehicle from the initial position and attitude of the vehicle at predetermined update cycles; a speed planning unit that sets a candidate position series, which is a series of candidate positions of the vehicle within a predicted target time range longer than the future update cycle, based on the future position and attitude of the vehicle, and determines a planned speed of the vehicle that, when used for driving the vehicle, will prevent the obstacles from entering the safety area over the predicted target time range and allow the vehicle to drive with a small deviation from the target path; and a generation unit that generates a planned speed series, which is a series of the planned speeds for each update cycle. [Effects of the Invention]

[0007] According to this 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 the target path, while ensuring that obstacles do not enter a safety area set according to the vehicle's speed. [Brief explanation of the drawing]

[0008] [Figure 1] Figure 1 is a schematic diagram illustrating an example of a safety zone. [Figure 2] Figure 2 is a diagram showing the hardware configuration of the travel planning device. [Figure 3] Figure 3 is a configuration diagram of the travel planning device according to the present embodiment. [Figure 4] Figure 4 is an example of a grid representing a candidate position sequence to be set. [Figure 5] Figure 5 is a diagram for explaining an example of setting a candidate position sequence. [Figure 6] Figure 6 is a diagram for explaining the calculation of an obstacle term. [Figure 7] Figure 7 is a diagram for explaining the calculation of an obstacle term considering a safety area. [Figure 8] Figure 8 is a flowchart showing the flow of travel planning processing by the travel planning device according to the first embodiment. [Figure 9] Figure 9 is a flowchart showing the flow of speed planning processing according to the first embodiment. [Figure 10] Figure 10 is a diagram showing an example of a travel image of a vehicle corresponding to travel planning processing by the travel planning device. [Figure 11] Figure 11 is an example of an image when the shape of the vehicle is represented by a circle. [Figure 12] Figure 12 is an image diagram of candidate states according to an objective function and constraints. [Figure 13] Figure 13 is an image diagram of obstacle determination. [Figure 14] Figure 14 is an image diagram of obstacle determination considering a safety area. [Figure 15] Figure 15 is a flowchart showing the flow of travel planning processing by the travel planning device according to the second embodiment. [Figure 16] Figure 16 is a flowchart showing the flow of speed planning processing according to the second embodiment. [Figure 17] Figure 17 is a diagram for explaining a method when the shape of the vehicle is represented by a plurality of circles. [Figure 18] Figure 18 is a diagram for explaining a method when the shape of the vehicle is represented by a plurality of circles. [Figure 19] FIG. 19 is a diagram for explaining a setting mode of a candidate position sequence in a modification example. [Figure 20] FIG. 20 is an image diagram when v and ω are obtained by DWA. [Figure 21] FIG. 21 is an image diagram when v and ω are obtained by MPC. [Figure 22] FIG. 22 is an experimental example in which the method of the present embodiment is applied by expressing the shape of a vehicle with a plurality of circles.

MODE FOR CARRYING OUT THE INVENTION

[0009] Hereinafter, an example of an embodiment of the present invention will be described while referring to the drawings. In each of the drawings, the same or equivalent components and parts are given the same reference numerals. Also, the dimensional ratios in the drawings are exaggerated for the convenience of explanation and may be different from the actual ratios.

[0010] The present disclosure relates to a travel plan of a vehicle as a movable robot. The "travel plan" referred to here is a travel plan generated by simulation before the vehicle travels, and the actual vehicle travels according to the travel plan. In the travel plan of the present disclosure, the travel of a vehicle is planned in which a safety region having a length that increases as the translational speed of the vehicle increases from the front end of the vehicle toward the front and a width approximating the width of the vehicle is set. Here, the front refers to the direction on the traveling direction side of the vehicle, and generally, the direction that coincides with the traveling direction of the vehicle is preferable, but in the case of a vehicle with a steering mechanism, it may be substituted with the direction on the traveling direction side based on the posture of the vehicle. Note that the width of the safety region may be wider than the width of the vehicle within a range suitable for the purpose of enabling the vehicle to travel without interfering with obstacles. For example, if the width of the safety region is several times the width of the vehicle, it becomes difficult to travel without interfering with obstacles, so such a large size is not included. Note that the shape of the safety region and the vehicle used in the present embodiment can be constituted by any of a circular shape, a rectangular shape, an elliptical shape, and a polygonal shape.

[0011] In this embodiment, the map information showing the placement of obstacles, the safety area, and the shape of the vehicle are described as planar projections, but they are not limited to this and may be treated three-dimensionally, including the height direction. In that case, the shape of the safety area and the vehicle can be composed of a sphere, a rectangular prism, a cube, etc. Furthermore, in the calculation to ensure that the safety area and the vehicle do not interfere with obstacles, the distance in the height direction is also included in the calculation.

[0012] The safety zone will now be explained. Figure 1 is a schematic diagram illustrating an example of a safety zone. The safety zone may be determined in advance, for example, from information on safety standards and the shape of the vehicle, or it may be determined from the distance required to decelerate and stop the vehicle at a given speed. The safety zone is defined using the length of the upper safety zone, the upper speed limit, the length of the lower safety zone, and the lower speed limit.

[0013] For example, suppose the upper limit of the safe zone is defined as 1.6m in length, 1.8m / sec in speed, 0.35m in length, and 0.3m / sec in speed. In this case, the length Y of the safe zone at any given translational speed can be calculated linearly using the following formula, based on the upper and lower limits.

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

[0015] However, v is an arbitrary translational velocity. The vertical width of the safety area is determined by the above formula, and the horizontal width is determined from the width of the vehicle's shape. The method of calculating the safety area as a circle will be described later.

[0016] As representative examples of the processing for the speed command generation system related to the vehicle's driving plan, DWA (Dynamic Window Approach), a search-based method, and MPC (Model Predictive Control), an optimization-based model prediction method, can be adopted. Therefore, the embodiment using DWA will be described as the first embodiment, and the embodiment using MPC will be described as the second embodiment. Furthermore, other modifications will be described after the first and second embodiments.

[0017] This section describes search-based and optimization-based methods. The search-based method divides space into a grid based on translational and angular velocities and calculates the sequence of translational and angular velocities that approaches the goal in the shortest time without interference. The optimization-based method expresses the objective function and constraints mathematically and uses an optimization technique to calculate the sequences of translational and angular velocities. Translational velocity is a scalar representing the change in position over time, while angular velocity represents the change in attitude over time.

[0018] First, the functional and hardware configurations common to each embodiment will be described. Figure 2 is a diagram showing the hardware configuration of the travel planning device. Figure 3 is a configuration diagram of the travel planning device according to this embodiment.

[0019] As shown in Figure 2, the travel planning device 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 interface 44. Each component is connected to the others via a bus 46 so as to be able to communicate with each other.

[0020] The storage device 36 stores a driving plan program for executing driving plan processing. The CPU 32 is a central processing unit that executes various programs and controls each component. Specifically, the CPU 32 reads the program from the storage device 36 and executes the program using memory 34 as a workspace. The CPU 32 controls each component and performs various calculations according to the program stored in the storage device 36.

[0021] Memory 34 consists of RAM (Random Access Memory) and temporarily stores programs and data as a working area. Storage device 36 consists of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including the operating system and various data.

[0022] The drive mechanism 38 is a power source such as a motor and battery, a transmission, tires, and other mechanisms for driving and operating the vehicle 20. The sensors 40 detect the state of the vehicle 20, such as its position, attitude, speed, and acceleration, and include, for example, a gyro sensor, a speed sensor, a camera, etc.

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

[0024] As shown in Figure 3, the driving plan device 30 is assumed to be a device installed inside the vehicle 20. However, the configuration is not limited to having the driving plan device 30 installed inside the vehicle 20; it may also be installed externally and transmit driving instruction data to the vehicle 20.

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

[0026] As shown in Figure 3, the travel planning device 30 includes, as a functional configuration, 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 configuration is realized when the CPU 32 reads the travel planning program stored in the storage device 36, expands it into the memory 34, and executes it. Furthermore, since the control contents of each part of the travel planning device 30 are different in each embodiment, the code for each part of the travel planning device 30 in the first embodiment is denoted with "A," and the code for each part of the travel planning device 30 in the second embodiment is denoted with "B" to distinguish them.

[0027] The execution control unit 100 repeatedly executes the initial processing unit 102 and the acquisition unit 104 before the vehicle 20 starts moving, and then executes the update unit 106 and the speed planning unit 108. In the following, the vehicle 20 may also be simply referred to as the vehicle. The vehicle's driving mechanism may be a differential two-wheel mechanism, an automobile mechanism (steering mechanism), or an omnidirectional movement mechanism.

[0028] The initial processing unit 102 acquires map information indicating the arrangement of obstacles, and a target route set so that the vehicle body does not interfere with obstacles during its journey. The map information may be stored in advance in the storage device 36, for example, or it 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).

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

[0030] The update unit 106 calculates the future position and attitude of the vehicle at predetermined update cycles based on the initial position and attitude of the vehicle acquired by the acquisition unit 104. This "calculation" is referred to here as "update". The initial point in time of the update cycle (t=0) is the first point in time (t=0), and the future point in time is a point in time later than the initial point in the update cycle (t>0). Specifically, the update unit 106 defines the future position and attitude of the vehicle after the update as the position and attitude of the vehicle after traveling at a planned speed corresponding to the future position and attitude of the vehicle for the duration of the update cycle. The future position and attitude of the vehicle can be determined by the following formula, where t is the future time, dt is the update cycle, v is the translational speed, and θ is the attitude.

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

[0032] The speed planning unit 108 sets a candidate position sequence, which is a series of candidate vehicle positions within a predicted time range longer than the future update cycle, based on the future position and attitude of the vehicle. Then, based on the map information, target route, and candidate position sequence, the speed planning unit 108 determines a predicted speed sequence, which is a series of predicted speeds that, when used for vehicle travel, would prevent obstacles from entering the safety area over the predicted time range and allow the vehicle to travel with minimal deviation from the target route. Based on the predicted speed sequence, the unit determines the planned speed. The predicted speed and planned speed are expressed in terms of translational speed and angular velocity.

[0033] The generation unit 110 generates a planned speed series, which is a series of planned speeds for each update cycle.

[0034] The following describes each embodiment. [First Embodiment] The first embodiment will now 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 target route described above. The target translational speed may be, for example, a trapezoidal speed curve in which the vehicle accelerates from the starting position, maintains a constant speed thereafter, and then decelerates before reaching the goal, or the maximum speed according to the vehicle's specifications.

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

[0036] (1) The setting process will be explained. The speed planning unit 108A performs the process of setting multiple discrete combinations of possible translational speeds and possible angular velocities. The possible translational speeds and possible angular velocities remain constant throughout the predicted time range. In addition, for each combination of possible translational speeds and possible angular velocities, the speed planning unit 108A performs the process of setting a possible translational speed series, which is a series of possible translational speeds, and a possible angular velocity series, which is a series of possible angular velocities.

[0037] Here, an example of the configuration of the candidate position series set by the speed planning unit 108A will be described. Figure 4 is 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, indicating the vehicle's speed at a certain point in the future. a is the maximum value of acceleration, and α is 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 speeds and angular velocities that the vehicle 20 can take at the next time (a time dt advanced from the future time) based on the acceleration and angular acceleration with the vehicle's speed as the center. If the upper or lower limit of v and ω is reached, the speed planning unit 108A terminates the range of the candidate position series at the upper or lower limit. 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 speed planning unit 108A divides the range into a grid 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. The maximum and minimum values ​​of V and ω, as well as the maximum and minimum acceleration and angular acceleration values, are determined, for example, from the specifications of the motor of the vehicle 20.

[0038] (2) The calculation process will be explained. As a preprocessing step, the speed planning unit 108A sets a series of vehicle positions within the predicted time range as a candidate position series for each set combination, based on the future vehicle position and attitude, possible translational speed series, and possible angular speed series. If necessary for the calculation of the obstacle term described later, an attitude series may also be set as this 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 speed and the target translational speed corresponding to the future vehicle position. Furthermore, the speed planning unit 108A finds a predicted translational speed series and a predicted angular speed series that minimize the cost, based on map information, provided that the vehicle body and safety area at all positions in the candidate position series do not interfere with obstacles. The speed planning unit 108A then sets the translational and angular velocities that constitute the obtained predicted translational velocity series and predicted angular velocity series as the planned speeds.

[0039] Specifically, the speed planning unit 108A calculates the cost using a cost function. The cost function is expressed by equation (1) below.

[0040] cost=α*path+β*vel+γ*obstancle ...(1)

[0041] In the cost function, path represents the tracking term, vel represents the velocity term, and obstacle represents the obstacle term, while α, β, and γ are weights. Figure 5 is a diagram illustrating an example of setting up candidate position sequences. The velocity planning unit 108A calculates the predicted trajectory for each of the candidate position sequences after time tn, assuming constant velocity based on the velocity vector (v, ω). The velocity vector (v, ω) is represented as a vector representing each point of the possible translational velocity v and possible angular velocity ω. In this way, the velocity planning unit 108A sets up candidate position sequences, which are sequences of vehicle positions within the prediction target time range. However, as mentioned above, the prediction target time range is longer than the future update cycle.

[0042] The speed planning unit 108A performs the following first to fifth calculation processes as part of the cost calculation process. As the first calculation process, the speed planning unit 108A calculates the tracking term. It finds the perpendicular line between the end of the predicted trajectory and the target path and uses the distance between them as the value of the tracking term. The tracking term is a term that indicates whether the vehicle is close to the target path after tn time. As the second calculation process, the speed planning unit 108A calculates the speed term. The absolute value of the difference between the possible translational speed and the target translational speed is used as the speed term. In other words, it is a term that indicates whether the target translational speed is being achieved. The target translational speed at this time is the target translational speed at a position close to the future vehicle position updated by the update unit 106, among the target translational speeds at each position on the target path acquired by the initial processing unit 102. As the third calculation process, the speed planning unit 108A calculates the 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 velocity and predicted angular velocity that minimizes the cost.

[0043] The calculation of the obstacle term will now be explained. Figure 6 is a diagram illustrating the calculation of the obstacle term. Figure 7 is a diagram illustrating the calculation of the obstacle term considering the safety area. R represents the position in the candidate position sequence at each time step, S represents the safety area at each time step, and O in each circle represents the detection point of the obstacle. 6A represents the location where the obstacle is interfering. In the calculation of the obstacle term, the first step is to calculate the vehicle's position and orientation at each future time step. In the second step, it is determined whether the shape of the vehicle in the candidate position sequence, based on the vehicle's position and orientation at each future time step, is interfering with the obstacle. Also, in the second step, when determining whether the safety area is being considered, it is assumed that there is interference if a point of the obstacle is within the safety area and the shape of the vehicle. 7A represents the location where the obstacle, the vehicle in the candidate position sequence, and the safety area are interfering. In the third step, if there is interference, a large fixed value is used as the value of the obstacle term. If there is no interference, the distance between the vehicle's origin and the obstacle at the closest point is calculated at each time step, and the reciprocal of this distance is used as the value of the obstacle term. For example, the fixed value when there is interference is larger than the reciprocal value when there is no interference. In addition, in equation (1), it is also possible to perform calculations using only the tracking term and velocity term as the cost function without using the obstacle term, and not adopting the velocity vector (v, ω) where the predicted trajectory interferes with the obstacle as a combination of predicted translational velocity and predicted angular velocity.

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

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

[0046] (Process flow) Figure 8 is a flowchart showing the flow of the driving plan processing by the driving plan device 30 according to the first embodiment. Figure 9 is a flowchart showing the flow of the speed planning processing according to the first embodiment. Figure 10 is a diagram showing an example of the driving image of the vehicle 20 corresponding to the driving plan processing by the driving plan device 30.

[0047] When the vehicle 20 is powered on, the CPU 32 reads the driving plan program from the storage device 36, loads it into memory 34, and executes it. As a result, the CPU 32 functions as one of the various functional configurations of the driving plan device 30, and the driving plan processing shown in Figure 8 is executed. In the execution of the driving plan program, first, the execution control unit 100A instructs the initial processing unit 102A to start.

[0048] In step S10, the initial processing unit 102A acquires map information indicating the placement of obstacles and a target route set so that the vehicle body does not interfere with obstacles during its journey. An image of the vehicle 20 in step S10 is shown in the left diagram of Figure 10.

[0049] In step S12, the acquisition unit 104A acquires the initial position and attitude of the vehicle through sensing. The image of the vehicle 20 in step S12 is shown in the left diagram of Figure 10.

[0050] In step S14, the speed planning unit 108A sets candidate position sequences and determines the planned speed that minimizes cost. The image of the vehicle 20 and the safety area S in step S14 is shown in the right-hand figure of Figure 10.

[0051] In step S16, the update unit 106A determines the future position and attitude of the vehicle after the update, which is the position and attitude of the vehicle after the update, based on the future position and attitude of the vehicle if it were to travel for the duration of the update cycle at the planned speed calculated by the speed planning unit 108A. An image of the vehicle 20 and the safety area S in step S16 is shown in the right-hand figure of Figure 10.

[0052] In step S18, the speed planning unit 108A determines whether the updated future position of the vehicle is at the end of the route to be planned (i.e., the target route). If it determines that the vehicle's position is not at the end of the route (negative determination), it returns to step S14 and repeats the process. On the other hand, if it determines that the vehicle's position is at the end of the route (positive determination), it proceeds to step S20. The image of the vehicle 20 and the safety area S in step S18 is shown in the right-hand figure of Figure 10.

[0053] In step S20, the generation unit 110A generates a planned speed series, which is a series of planned speeds for each update cycle, from the planned speeds repeatedly calculated by the speed planning unit 108A for each update cycle, and ends the series of processes by this driving plan program. Here, an example of a planned speed series of planned speeds (v, ω) used until the vehicle 20 reaches the target point is shown in Table 1.

[0054] [Table 1]

[0055] Next, with reference to Figure 9, the subroutine for the speed planning process executed by the speed planning unit 108A in step S14 will be described.

[0056] In step S30, the speed planning unit 108A sets multiple discrete combinations of possible translational velocities and possible angular velocities.

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

[0058] In step S34, the speed planning unit 108A uses the cost function of equation (1) above to calculate the follow term, the speed term, and the obstacle term for each set combination, and calculates the cost.

[0059] In step S36, the speed planning unit 108A determines the predicted translational velocity series and the predicted angular velocity series that minimize costs, sets the translational and angular velocities constituting the determined predicted translational velocity series and predicted angular velocity series as the planned speeds, and returns to step S16 in Figure 8.

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

[0061] [Second Embodiment] A second embodiment will be described. The speed planning unit 108B performs the following processes: (1) acquiring a target position sequence generated based on the target route and for each update cycle; (2) setting a command sequence which is a candidate translational speed sequence and a candidate angular speed sequence for each update cycle over the predicted time range based on the future position and attitude of the vehicle; (3) setting a sequence of vehicle positions based on the command sequence as a candidate position sequence; (4) determining a predicted translational speed sequence and a predicted angular speed sequence from the command sequence, on the condition that the vehicle body and safety area at all positions in the candidate position sequence do not interfere with obstacles based on map information, and minimizing the evaluation value including the amount of error between the target position sequence and the candidate position sequence over the predicted time range; and (4) setting the translational speed and angular speed included in the predicted translational speed sequence and the predicted angular speed sequence, which correspond to the future position and attitude of the vehicle, as the planned speed. In other words, the processing of the speed planning unit 108B includes (1) acquiring a target position sequence, (2) calculating a candidate position sequence, and (3) calculating the planned speed.

[0062] (1) The process of obtaining the target position sequence will be explained. The speed planning unit 108B obtains the target position sequence, which is a sequence of positions for each update cycle generated based on the target path. The target position sequence is x ref , y ref Let's assume that the target posture is θ.ref Let's assume that.

[0063] (2) The process for calculating the candidate position sequence will be explained. 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. The command sequence consists of a candidate translational speed sequence, which is a sequence of candidate translational speeds for each update cycle over the predicted time range, and a candidate angular velocity sequence, which is a sequence of candidate angular velocities.

[0064] The candidate translational velocity series and candidate angular velocity series are calculated as follows, for example: The candidate translational velocity v is calculated using an equation such as future translational velocity + acceleration × dt (predicted time t1, t2, tn). The candidate angular velocity ω is θ ref (k+i)-θ ref It is calculated using an expression like (k+i-1) / dt.

[0065] (3) The process for calculating the planned speed will be explained. The speed planning unit 108B determines the predicted translational speed and predicted angular speed based on a command sequence that minimizes a predetermined evaluation value. The evaluation value is conditional on ensuring that the vehicle and safety area at all positions in the candidate position sequence are not interfered with by obstacles, based on map information. The evaluation value also includes the amount of error between the target position sequence and the candidate position sequence over the predicted time range. At least position is required as the object of the error amount, and attitude may also be included as the object of the error amount. The direction of travel and attitude can be uniquely determined from the position and vehicle kinematics.

[0066] The objective function and constraints for calculating the evaluation value will be explained.

[0067] The objective function is expressed by equation (2) below.

number

Equation

Equation

[0068] Note that Np: prediction horizon, Nc: control horizon, Nc < Np, P ∈ R 3×3 , Q ∈ R 3×3 , R ∈ R 3×3 and

[0069] . The constraints are represented by the following equations (3-1) to (3-4).

Equation

Equation

Equation

Equation

[0070] Figure 11 shows an example of how the shape of a vehicle can be represented by circles. Using a circumscribed circle, as in 10A, has the advantage of lower computational load. Using multiple circles, as in 10B, has the advantage of reducing wasted space in the representation of the vehicle's shape.

[0071] Figure 12 is an illustrative diagram of the candidate position sequence based on the objective function and constraints. The acquired vehicle position is shown as x and y, and the target path is position x. r , y r This indicates the trajectory of the reference path.

[0072] Figure 13 is an illustrative diagram of the obstacle detection process. In the detection process, a circumscribed circle is set for the shape of the vehicle in the candidate position sequence. In the calculation of the constraints in equation (3-4), the distance to the obstacle is calculated for each position in the candidate position sequence, and the variables are updated so that this distance is greater than or equal to the radius of the circumscribed circle. In this way, constraints are calculated to prevent interference with obstacles, and these are reflected in the calculation of the objective function.

[0073] Figure 14 is an illustrative diagram of obstacle detection considering the safety zone. Multiple circles are set for the safety zone of the candidate position sequence and the shape of the vehicle. In calculating the constraint in equation (3-4), the distance to all obstacles is calculated for each of the multiple circles at each position in the candidate position sequence, and the variables are updated so that the calculated distance is greater than or equal to the radius of each of the multiple circles. For example, if obstacle constraints are set with multiple circles, the inequality constraint in equation (3-4) increases by the number of circles.

[0074] (Process flow) Figure 15 is a flowchart showing the flow of the travel planning process by the travel planning device 30 according to the second embodiment. Figure 16 is a flowchart showing the flow of the speed planning process according to the second embodiment.

[0075] When the vehicle 20 is powered on, the CPU 32 reads the driving plan program from the storage device 36, loads it into memory 34, and executes it. As a result, the CPU 32 functions as one of the various functional configurations of the driving plan device 30, and the driving plan processing shown in Figure 15 is executed. In the execution of the driving plan program, first, the execution control unit 100B instructs the initial processing unit 102B to start.

[0076] In step S110, the initial processing unit 102B acquires map information indicating the placement of obstacles, and a target route set so that the vehicle body does not interfere with obstacles during its journey.

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

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

[0079] In step S116, the update unit 106B determines the future position and attitude of the vehicle after the update, which is the position and attitude of the vehicle after the update, based on the future position and attitude of the vehicle if it were to travel for the duration of the update cycle at the planned speed calculated by the speed planning unit 108B.

[0080] In step S118, the speed planning unit 108B determines whether the updated future position of the vehicle is at the end of the route to be planned (i.e., the target route). If it determines that the vehicle's position is not at the end of the route (negative determination), the process returns to step S114 and is repeated. On the other hand, if it determines that the vehicle's position is at the end of the route (positive determination), the process proceeds to step S120.

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

[0082] Next, with reference to Figure 16, the subroutine for the speed planning process executed by the speed planning unit 108B in step S114 will be described.

[0083] In step S130, the speed planning unit 108B obtains a target position sequence, which is a sequence of positions generated based on the target path.

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

[0085] In step S134, the speed planning unit 108B uses the objective function in equation (2) and the constraints in equations (3-1) to (3-4) above to determine a predicted translational speed sequence and a predicted angular velocity sequence based on a command sequence that minimizes the evaluation value. The unit sets the translational speed and angular velocity included in the determined predicted translational speed sequence and predicted angular velocity sequence, relative to the future position and attitude of the vehicle, as the planned speed, and returns to step S116 in Figure 15.

[0086] As described above, according to the driving plan device of this embodiment, by performing a simulation using MPC, which is an example of a speed command generation system process, before the vehicle is driven, it is possible to create a driving plan that prevents obstacles from entering the safety area set according to the vehicle's speed, while suppressing deviation from the target path.

[0087] (A method of representing the shape of a vehicle using multiple circles) This section provides a supplementary explanation of the specific method for representing the shape of a vehicle using multiple circles. Figures 17 and 18 illustrate the method for representing the shape of a vehicle using multiple circles. The multiple circles are set up following steps (A1) to (A5). In (A1), a square is calculated using the length of the shorter side when the shape of the vehicle is approximated as a rectangle. In (A2), the number of squares that can fill the shape of the vehicle is calculated. For example, squares are added from the left end until the length of the added squares exceeds the shape of the vehicle. This number of squares becomes the number of circles. In (A3), squares that jump over the shape of the vehicle are deleted and new squares are added from the right end. This is to reduce the excess when covering the shape of the vehicle with squares. In (A4), the distance between the squares at both ends is calculated and divided by the number of squares other than those at both ends + 1. Based on this value, the positions of the squares are adjusted so that they are equally spaced. In (A5), the circumcircle of each square is calculated. In this way, the circumcircle is set for each of the multiple squares arranged in the length direction of the shape of the vehicle. When controlling the vehicle's attitude based on multiple defined circles, the center coordinates of each circumscribed circle are rotated according to the vehicle's attitude.

[0088] [Example 1] Modification 1 describes a configuration in which only the translational velocity v is controlled in the first embodiment, and the angular velocity ω is calculated and set from the translational velocity v and the target path.

[0089] The speed planning unit 108A performs the following processes: (1) setting multiple discrete possible translational speeds that are constant over the prediction target time range; (2) setting a possible translational speed sequence for each possible translational speed; (3) setting a candidate position sequence for each possible translational speed, based on the future vehicle position, target path, and possible translational speed sequence, and calculating the cost including the error between the end of the candidate position sequence and the target path and the error between the possible translational speed and the target translational speed corresponding to the future vehicle position; (4) finding a predicted translational speed sequence, which is a sequence of predicted translational speeds that minimizes the cost among the possible translational speed sequences, based on map information, on the condition that the vehicle body and safety area at all positions in the candidate position sequence do not interfere with obstacles; and (5) setting the translational speeds constituting the predicted translational speed sequence, and the angular velocity determined based on the translational speed and target path, as the planned speed. As described above, the possible translational speed is a constant value over the time range to be predicted. The candidate position sequence is a sequence of the future vehicle position, the target path, and the vehicle's position on the target path within the time range to be predicted, based on the possible translational speed sequence, for each possible translational speed. Next, the speed planning unit 108A performs the process of (3) determining the predicted translational speed. The predicted translational speed is determined based on map information, on the condition that the vehicle and safety area at all positions in the candidate position sequence do not interfere with obstacles, such that the difference between the possible translational speed and the target translational speed corresponding to the future vehicle position is minimized. The speed planning unit 108A then performs the process of (4) determining the angular velocity based on the predicted translational speed and the target path.

[0090] Figure 19 illustrates the setting method of the candidate position sequence in the modified example. As a prerequisite, the vehicle's position and orientation (x, y, θ) are determined using self-localization. Based on the acceleration centered on the vehicle's velocity, the range of possible translational velocities that vehicle 20 can take at the next time (time dt after the current time) is calculated. Within that range, the possible translational velocities are divided into specific sampling intervals. For each divided possible translational velocity, the predicted trajectory for tn time, assuming constant velocity, is calculated along the target path. The cost of the DWA cost function is calculated, and the v (predicted translational velocity) with the lowest cost is determined, taking into account the safety region. The angular velocity ω (predicted angular velocity) is determined so that the vehicle is located on the target path when traveling for dt time at the determined predicted translational velocity.

[0091] The processing flow for Modification 1 can be performed by replacing the flowchart in Figure 9 described in the First Embodiment. In step S30, the speed planning unit 108A sets multiple discretely prepared possible translational speeds. In step S32, for each possible translational speed, the speed planning unit 108A sets a candidate position sequence, which is a sequence of vehicle positions on the target path within the predicted time range, based on the future vehicle position, target path, and possible translational speed sequence. In step S34, the speed planning unit 108A calculates the cost using the cost function of equation (1) above and determines the predicted translational speed such that the difference with a predetermined target translational speed is minimized. In step S36, the speed planning unit 108A sets the translational speeds that constitute the predicted translational speed, and the angular speed determined based on the translational speeds and target path, as the planned speed.

[0092] [Differentiation 2] Modification 2 is a method for determining the velocity vector (v,ω) by reducing the error between the position of the end of the predicted trajectory and the far position ahead of the vehicle on the target path. The cases in which this is applied to the DWA of the first embodiment and the MPC of the second embodiment will be described separately.

[0093] Figure 20 is an illustrative diagram of how v and ω are determined using DWA. In the example shown in Figure 20, v and ω are selected to minimize the error between the intersection point PA with the target path, which is a constant distance from the vehicle, and the end of the predicted trajectory. The constant distance can be assumed to be a rectangle as shown in Figure 20, or a circle. The position of the intersection point PA should be set to a position far greater than the position that can be reached after tn seconds. Then, because the vehicle will travel towards that distant position, it can smoothly return to the target path if it deviates from the target path due to slippage or other reasons. In this case, the velocity term does not need to be used in equation (1). In this case, the translational velocity v will be the maximum translational velocity v among the possible translational velocities within the range where the safety region does not interfere.

[0094] Figure 21 is an illustrative diagram of how v and ω are determined using MPC. For the objective function in equation (2) above, which selects vω that minimizes the error between the intersection point with the target path at a constant distance from the vehicle and the end of the predicted horizon, only the first term is used, and the second and third terms do not need to be used. This is because the position and orientation of points along the trajectory can be uniquely determined due to the kinematic inequality constraints.

[0095] Figure 22 shows an experimental example in which the method of this embodiment is applied by representing the shape of a vehicle with multiple circles. In the simulator environment, the shape of the vehicle was represented with multiple circles and set as an obstacle constraint, and it was confirmed that the vehicle could pass through a passage with obstacles.

[0096] Furthermore, the information processing that the CPU reads and executes in each of the above embodiments may be executed by various processors other than the CPU. Examples of such processors include PLDs (Programmable Logic Devices) such as FPGAs (Field-Programmable Gate Arrays) whose circuit configuration can be changed after manufacturing, and dedicated electrical circuits that are processors with circuit configurations specifically designed to execute specific processing, such as ASICs (Application Specific Integrated Circuits). In addition, the information processing may be executed by one of these various processors, or by a combination of two or more processors of the same or different types (for example, multiple FPGAs, and a combination of a CPU and an FPGA). More specifically, the hardware structure of these various processors is an electrical circuit that combines circuit elements such as semiconductor elements.

[0097] Furthermore, while the above embodiments describe a configuration in which the information processing program is pre-stored (installed) in ROM or storage, the invention is not limited to this. 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), DVD-ROM (Digital Versatile Disk Read Only Memory), or USB (Universal Serial Bus) memory. Alternatively, the program may be provided in a form that is downloaded from an external device via a network.

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

[0099] (Additional note 1) A vehicle travel planning device (30) is set in which a safety area is set that has a length that increases as the translational speed of the vehicle increases and a width that approximates the width of the vehicle, extending forward from the front end of the vehicle, An initial processing unit (102) acquires map information showing the arrangement of obstacles and a target path set so that the vehicle body does not interfere with the obstacles during the vehicle's journey. An acquisition unit (104) that acquires the initial position and orientation of the vehicle, An update unit (106) updates the future position and orientation of the vehicle at predetermined update intervals, based on the initial position and orientation of the vehicle, A speed planning unit (108) sets a candidate position sequence, which is a series of candidate positions for the vehicle within a predicted target time range longer than the future update cycle, based on the future position and orientation of the vehicle, and determines a planned speed for the vehicle that, when used for driving the vehicle, will prevent the obstacle from entering the safety area over the predicted target time range and minimize deviation from the target route. A generation unit (110) that generates a planned speed series, which is a series of planned speeds for each update cycle, A travel planning device (30) equipped with the following. (Additional note 2) The update unit (106) determines the future position and orientation of the vehicle after updating, which is the position and orientation of the vehicle after updating, based on the future position and orientation of the vehicle and the vehicle traveling at the planned speed corresponding to the future position and orientation of the vehicle for the duration of the update cycle. The travel planning device described in Appendix 1. (Additional note 3) The initial processing unit (102) further acquires or sets the target translational velocity at each position on the target path. The speed planning unit (108) performs a process of setting multiple discrete combinations of possible translational velocities and possible angular velocities that are constant over the predicted time range, For each of the above combinations, a process is performed to set the possible translational velocity series, which is the sequence of possible translational velocities, and the possible angular velocity series, which is the sequence of possible angular velocities. For each of the aforementioned combinations, a sequence of vehicle positions is set as the candidate position sequence based on the future position and attitude of the vehicle, the possible translational speed sequence, and the possible angular velocity sequence; and a cost is calculated including the error between the end of the candidate position sequence and the target path, and the error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle. A process to determine a predicted translational speed sequence and a predicted angular velocity sequence, which are sequences of predicted translational speeds and predicted angular velocity sequences, from among the possible translational speed sequence and the possible angular velocity sequence, based on the map information, such that the cost is minimized on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacle. The process involves setting the translational speed and angular velocity constituting the predicted translational speed sequence and the predicted angular speed sequence as the planned speed. A travel planning device as described in Appendix 1 or Appendix 2. (Additional note 4) The initial processing unit (102) further acquires or sets the target translational velocity at each position on the target path. The speed planning unit (108) includes a process for setting multiple discrete possible translational speeds that are constant over the predicted time range, For each of the aforementioned possible translational speeds, a process is performed to set a possible translational speed sequence, which is a sequence of the aforementioned possible translational speeds. For each of the possible translational speeds, a process is performed to set a sequence of vehicle positions on the target path as the candidate position sequence based on the future position of the vehicle, the target path, and the sequence of possible translational speeds, and to calculate a cost that includes the error between the end of the candidate position sequence and the target path, and the error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle. A process to determine a predicted translational speed sequence, which is a sequence of predicted translational speeds from the aforementioned possible translational speed sequence, on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacle, based on the map information, and which minimizes the cost. The process involves setting the translational velocities constituting the predicted translational velocity sequence, and the angular velocity determined based on the translational velocities and the target path, as the planned velocity. A travel planning device as described in Appendix 1 or Appendix 2. (Additional note 5) The speed planning unit (108) performs a process to obtain a target position sequence, which is a sequence of positions for each update cycle generated based on the target path, A process to set a command sequence which is a candidate translational velocity sequence and a candidate angular velocity sequence, which is a sequence of candidate angular velocities, for each update cycle within the predicted target time range based on the future position and attitude of the vehicle, and to set a sequence of the vehicle's position based on the command sequence as the candidate position sequence, A process to determine a predicted translational velocity sequence and a predicted angular velocity sequence, which are sequences of predicted angular velocity, from the command sequence, on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacles, based on the map information, and which minimizes the evaluation value including the amount of error between the target position sequence and the candidate position sequence over the predicted target time range. The process involves setting the translational speed and angular velocity included in the predicted translational speed series and the predicted angular velocity series, which correspond to the future position and attitude of the vehicle, as the planned speed. A travel planning device as described in Appendix 1 or Appendix 2. (Additional note 6) The safety area and the shape of the vehicle for setting the safety area are configured as either a circle, a rectangle, or a polygon. A travel planning device as described in any one of the appendices 1 to 5. (Additional note 7) When the aforementioned shape is set as multiple circular shapes, the number of squares whose side length is the same as the shorter side length when the shape of the vehicle is a rectangle is calculated, and a circumscribed circle is set for each of the multiple squares arranged along the length direction of the shape of the vehicle. The travel planning device described in Appendix 6. (Additional note 8) A method for planning the movement of a vehicle, wherein a safety area is set in which the length of the area increases as the translational speed of the vehicle increases, and the width of the area approximates the width of the vehicle, extending forward from the front end of the vehicle, The system acquires map information showing the arrangement of obstacles and a target route set so that the vehicle body does not interfere with the obstacles during its journey. The initial position and orientation of the vehicle are obtained, The initial position and orientation of the vehicle are updated at predetermined update intervals to determine the future position and orientation of the vehicle. Based on the future position and orientation of the vehicle, a candidate position sequence is set, which is a series of candidate positions for the vehicle within a predicted time range longer than the future update cycle. Based on the map information, the target route, and the candidate position sequence, a planned speed for the vehicle is determined that, when used for the vehicle's travel, will prevent the obstacle from entering the safety area over the predicted time range and minimize deviation from the target route. The process of generating a planned speed sequence, which is a sequence of planned speeds for each update cycle, A method of planning a journey that is executed by a computer. (Additional note 9) A vehicle travel planning program is set in which a safety area is defined that has a length that increases as the translational speed of the vehicle increases, and a width that approximates the width of the vehicle, extending forward from the front end of the vehicle, The system acquires map information showing the arrangement of obstacles and a target route set so that the vehicle body does not interfere with the obstacles during its journey. The initial position and orientation of the vehicle are obtained, The initial position and orientation of the vehicle are updated at predetermined update intervals to determine the future position and orientation of the vehicle. Based on the future position and orientation of the vehicle, a candidate position sequence is set, which is a series of candidate positions for the vehicle within a predicted time range longer than the future update cycle. Based on the map information, the target route, and the candidate position sequence, a planned speed for the vehicle is determined that, when used for the vehicle's travel, will prevent the obstacle from entering the safety area over the predicted time range and minimize deviation from the target route. The process of generating a planned speed sequence, which is a sequence of planned speeds for each update cycle, A driving plan program to be executed by a computer. [Explanation of symbols]

[0100] 30. Travel planning device 100 Execution Control Unit 102 Initial Processing Unit 104 Acquisition Department 106 Update Department 108 Speed ​​Planning Department 110 Generation part

Claims

1. A vehicle travel planning device is provided in which a safety area is set that has a length that increases as the translational speed of the vehicle increases, and a width that approximates the width of the vehicle, extending forward from the front end of the vehicle, An initial processing unit that acquires map information showing the arrangement of obstacles and a target path set so that the vehicle body does not interfere with the obstacles during the vehicle's journey, An acquisition unit that acquires the initial position and orientation of the vehicle, An update unit that updates the future position and orientation of the vehicle at predetermined update intervals, based on the initial position and orientation of the vehicle, A speed planning unit sets a candidate position sequence, which is a series of candidate positions for the vehicle within a predicted target time range longer than the future update cycle, based on the future position and orientation of the vehicle, and determines a planned speed for the vehicle that, when used for driving the vehicle, will prevent the obstacle from entering the safety area over the predicted target time range and minimize deviation from the target route. A generation unit that generates a planned speed series, which is a series of planned speeds for each update cycle, A travel planning device equipped with a travel planning system.

2. The update unit determines the future position and orientation of the vehicle after the update, which is the position and orientation of the vehicle after the update, by determining the future position and orientation of the vehicle when the vehicle travels at the planned speed corresponding to the future position and orientation of the vehicle for the duration of the update cycle. A travel planning device according to claim 1.

3. The initial processing unit further acquires or sets the target translational velocity at each position on the target path. The speed planning unit includes a process for setting multiple discrete combinations of possible translational velocities and possible angular velocities that are constant over the predicted time range, For each of the above combinations, a process is performed to set the possible translational velocity series, which is the sequence of possible translational velocities, and the possible angular velocity series, which is the sequence of possible angular velocities. For each of the aforementioned combinations, a sequence of vehicle positions is set as the candidate position sequence based on the future position and attitude of the vehicle, the possible translational speed sequence, and the possible angular velocity sequence; and a cost is calculated including the error between the end of the candidate position sequence and the target path, and the error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle. A process to determine a predicted translational speed sequence and a predicted angular velocity sequence, which are sequences of predicted translational speeds and predicted angular velocity sequences, from among the possible translational speed sequence and the possible angular velocity sequence, based on the map information, such that the cost is minimized on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacle. The process involves setting the translational speed and angular velocity constituting the predicted translational speed sequence and the predicted angular speed sequence as the planned speed. A travel planning device according to claim 1 or claim 2.

4. The initial processing unit further acquires or sets the target translational velocity at each position on the target path. The speed planning unit includes a process for setting multiple discrete possible translational speeds that are constant over the predicted time range, For each of the aforementioned possible translational speeds, a process is performed to set a possible translational speed sequence, which is a sequence of the aforementioned possible translational speeds. For each of the possible translational speeds, a process is performed to set a sequence of vehicle positions on the target path as the candidate position sequence based on the future position of the vehicle, the target path, and the sequence of possible translational speeds, and to calculate a cost that includes the error between the end of the candidate position sequence and the target path, and the error between the possible translational speed and the target translational speed corresponding to the future position of the vehicle. A process to determine a predicted translational speed sequence, which is a sequence of predicted translational speeds from the aforementioned possible translational speed sequence, on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacle, based on the map information, and which minimizes the cost. The process involves setting the translational velocities constituting the predicted translational velocity sequence, and the angular velocity determined based on the translational velocities and the target path, as the planned velocity. A travel planning device according to claim 1 or claim 2.

5. The speed planning unit performs a process to obtain a target position sequence, which is a sequence of positions for each update cycle generated based on the target path. A process to set a command sequence which is a candidate translational velocity sequence and a candidate angular velocity sequence, which is a sequence of candidate angular velocities, for each update cycle within the predicted target time range based on the future position and attitude of the vehicle, and to set a sequence of the vehicle's position based on the command sequence as the candidate position sequence, A process to determine a predicted translational velocity sequence and a predicted angular velocity sequence, which are sequences of predicted angular velocity, from the command sequence, on the condition that the vehicle body and the safety area at all positions in the candidate position sequence do not interfere with the obstacles, based on the map information, and which minimizes the evaluation value including the amount of error between the target position sequence and the candidate position sequence over the predicted target time range. The process involves setting the translational speed and angular velocity included in the predicted translational speed series and the predicted angular velocity series, which correspond to the future position and attitude of the vehicle, as the planned speed. A travel planning device according to claim 1 or claim 2.

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

7. When the aforementioned shape is set as multiple circular shapes, the number of squares whose side length is the same as the shorter side length when the shape of the vehicle is a rectangle is calculated, and a circumscribed circle is set for each of the multiple squares arranged along the length direction of the shape of the vehicle. The travel planning device according to claim 6.

8. A method for planning the movement of a vehicle, wherein a safety area is set in which the length of the area increases as the translational speed of the vehicle increases, and the width of the area approximates the width of the vehicle, extending forward from the front end of the vehicle, The system acquires map information showing the arrangement of obstacles and a target route set so that the vehicle body does not interfere with the obstacles during its journey. The initial position and orientation of the vehicle are obtained, The initial position and orientation of the vehicle are updated at predetermined update intervals to determine the future position and orientation of the vehicle. Based on the future position and orientation of the vehicle, a candidate position sequence is set, which is a series of candidate positions for the vehicle within a predicted time range longer than the future update cycle. Based on the map information, the target route, and the candidate position sequence, a planned speed for the vehicle is determined that, when used for the vehicle's travel, will prevent the obstacle from entering the safety area over the predicted time range and minimize deviation from the target route. The process of generating a planned speed sequence, which is a sequence of planned speeds for each update cycle, A method of planning a journey that is executed by a computer.

9. A vehicle travel planning program is set in which a safety area is defined that has a length that increases as the translational speed of the vehicle increases, and a width that approximates the width of the vehicle, extending forward from the front end of the vehicle, The system acquires map information showing the arrangement of obstacles and a target route set so that the vehicle body does not interfere with the obstacles during its journey. The initial position and orientation of the vehicle are obtained, The initial position and orientation of the vehicle are updated at predetermined update intervals to determine the future position and orientation of the vehicle. Based on the future position and orientation of the vehicle, a candidate position sequence is set, which is a series of candidate positions for the vehicle within a predicted time range longer than the future update cycle. Based on the map information, the target route, and the candidate position sequence, a planned speed for the vehicle is determined that, when used for the vehicle's travel, will prevent the obstacle from entering the safety area over the predicted time range and minimize deviation from the target route. The process of generating a planned speed sequence, which is a sequence of planned speeds for each update cycle, A driving plan program to be executed by a computer.

Images