Travel controller, travel control method, and travel control program

Abstract

To allow a vehicle, which has a longer detection distance as a travel speed is larger, and is decelerated when an obstacle is detected within the detection distance, to travel while reducing an occurrence frequency of an event that the vehicle is abruptly decelerated because the obstacle has entered the detection distance.SOLUTION: A travel controller 30 of a vehicle for which a safety area whose length gets larger as a translational speed of the vehicle is higher and whose width is approximate to a width of the vehicle is set toward ahead from a front end of the vehicle 20 includes: an initial processing unit 102 that acquires a target route set so that a vehicle body does not interfere with an obstacle during travel of the vehicle; an acquisition unit 104 that acquires a vehicle condition which includes a position, a posture, a translational speed, and an angular speed of the vehicle, and disposition information on an obstacle around the vehicle; and an action planning unit 106 that sets a candidate condition which relates to at least a part of a future vehicle condition on the basis of the target route, and determines at least one of the translational speed and the angular speed of the vehicle so that the obstacle does not intrude into the safety area on the basis of the candidate condition.SELECTED DRAWING: Figure 3

Description

[Technical Field]

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

[0002] Patent Document 1 describes a device for controlling the travel of a vehicle equipped with a sensor that detects obstacles. This device controls the deceleration of the vehicle when the sensor detects an obstacle within a predetermined detection distance ahead of the vehicle. The detection distance increases as the vehicle speed increases. This makes it possible to prevent the vehicle from colliding with an obstacle regardless of the vehicle's speed. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Patent Publication No. 10-161745 Summary of the Invention [Problem to be solved by the invention]

[0004] One method for driving a vehicle without colliding with obstacles in an environment where the existence of roads is not assumed, such as within the premises of a facility, is to create a driving plan consisting of a target route and target speed that will prevent the vehicle from colliding with obstacles before driving, and then drive the vehicle according to that driving plan during driving.

[0005] When a vehicle is traveling according to a travel plan, the position of an obstacle may differ from that in the plan, or an error may occur in recognizing the vehicle's position, causing the vehicle to travel a route different from the target route, which could result in the vehicle colliding with an obstacle.In other words, when a vehicle is traveling according to a travel plan, it is necessary to detect obstacles in front of the vehicle and prevent a collision.

[0006] If the collision prevention method described in Patent Document 1 is used as is, the vehicle will only start to decelerate when an obstacle comes within the detection distance while driving, and will then continue to decelerate as it approaches the obstacle until the obstacle is outside the detection distance. This results in a sudden deceleration from the driving speed before the obstacle was detected.

[0007] The present disclosure aims to provide a driving control device, a driving control method, and a driving control program that, in a vehicle that has a longer detection distance the faster the driving speed is, and that decelerates when an obstacle is detected within that detection distance, reduce the frequency of occurrence of an incident in which an obstacle enters within the detection distance and the vehicle suddenly decelerates. [Means for solving the problem]

[0008] In order to achieve the above-mentioned object, the driving control device of the present disclosure is a driving control device 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 is approximate to the width of the vehicle, and is equipped with an initial processing unit that acquires a target route that is set so that the vehicle body does not interfere with obstacles while the vehicle is traveling, an acquisition unit that acquires vehicle states including the position, attitude, translational speed, and angular speed of the vehicle, as well as information on the location of the obstacles around the vehicle, and an operation planning unit that sets candidate states for at least a portion of the future vehicle states based on the target route, and determines at least one of the translational speed and angular speed of the vehicle based on the candidate states so that the obstacle does not enter the safety area. [Effects of the Invention]

[0009] According to the present disclosure, it is possible to provide a driving control device, a driving control method, and a driving control program that allow a vehicle to travel while reducing the frequency of occurrence of an incident in which an obstacle enters within the detection distance and the vehicle suddenly decelerates. [Brief explanation of the drawings]

[0010] [Figure 1] FIG. 1 is a schematic diagram for explaining an example of a safety region. [Figure 2]FIG. 2 is a diagram showing the hardware configuration of the driving control device. [Figure 3] FIG. 3 is a configuration diagram of the driving control device according to this embodiment. [Figure 4] FIG. 4 is an example of a grid representing the candidate states to be set. [Figure 5] FIG. 5 is a diagram illustrating an example of setting the candidate states. [Figure 6] FIG. 6 is a diagram for explaining the calculation of the obstacle term. [Figure 7] FIG. 7 is a diagram for explaining the calculation of the obstacle term taking into account the safety area. [Figure 8] FIG. 8 is a flowchart showing the flow of the cruise control process performed by the cruise control device according to the first embodiment. [Figure 9] FIG. 9 is a flowchart showing the flow of the operation planning process according to the first embodiment. [Figure 10] FIG. 10 shows an example of an image in which the shape of a vehicle is represented by a circle. [Figure 11] FIG. 11 is an image diagram of candidate states based on objective functions and constraints. [Figure 12] FIG. 12 is an image diagram of obstacle detection. [Figure 13] FIG. 13 is an image diagram of obstacle detection taking into account the safety area. [Figure 14] FIG. 14 is a flowchart showing the flow of the cruise control process performed by the cruise control device 30B according to the second embodiment. [Figure 15] FIG. 15 is a flowchart showing the flow of the operation planning process according to the second embodiment. [Figure 16] FIG. 16 is a diagram for explaining a method for expressing the shape of a vehicle using a plurality of circles. [Figure 17] FIG. 17 is a diagram for explaining a method for expressing the shape of a vehicle using a plurality of circles. [Figure 18] FIG. 18 is a diagram for explaining a setting mode of the candidate state in the modified example. [Figure 19]FIG. 19 is an image diagram of how v and ω are calculated using DWA. [Figure 20] FIG. 20 is an image diagram of how v and ω are calculated using MPC. [Figure 21] FIG. 21 shows an example of an experiment in which the shape of a vehicle is represented by a plurality of circles and the method of this embodiment is applied. DETAILED DESCRIPTION OF THE INVENTION

[0011] 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.

[0012] The present disclosure relates to driving control of a vehicle as a mobile robot. In the driving control of the present disclosure, a safety region is set, which has 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 that approximates the width of the vehicle. Here, "forward" refers to the direction toward which the vehicle is traveling. Generally, the direction is preferably the same as the direction of travel of the vehicle, but 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 width of the vehicle as long as it is appropriate for the purpose of enabling the vehicle to travel without interfering with obstacles. For example, a safety region width that is several times the width of the vehicle would make it difficult to travel without interfering with obstacles, so 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.

[0013] In this embodiment, the obstacle location information, 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 handled three-dimensionally, including the height direction. In this case, the safety area and the vehicle shape 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 the obstacle also includes the distance in the height direction.

[0014] The safety area will now be described. Fig. 1 is a schematic diagram for explaining 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 limit safety area, the upper limit speed, the length of the lower limit safety area, and the lower limit speed.

[0015] For example, let's say the upper limit safety zone length is 1.6m, the upper limit speed is 1.8m / sec, the lower limit safety zone length is 0.35m, and the lower limit speed is 0.3m / sec. In this case, the length Y of the safety zone at any translational speed can be calculated linearly using the upper and lower limit values ​​using the following formula: Y=(1.6-0.35) / (1.8-0.3)*(v-0.3)+0.35 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 width of the vehicle shape. The method for calculating the safety area as a circle will be described later.

[0016] As typical examples of the processing of a speed command generation system related to a vehicle operation 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.

[0017] We will explain the search method and the optimization method. The search method is a method that divides space into grids based on translational velocity and angular velocity, and calculates a series of translational and angular velocities that will approach the goal in the shortest time without interference. The optimization method is a method that expresses the objective function and constraints as mathematical formulas and calculates a series of translational and angular velocities using an optimization method. Note that the translational velocity is a scalar that represents the change in position over time. The angular velocity represents the change in attitude over time.

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

[0019] 2, the driving control 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 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.

[0020] The storage device 36 stores a driving control program for executing driving control 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.

[0021] The memory 34 is made up of RAM (Random Access Memory) and serves as a working area to temporarily store programs and data. The storage device 36 is made up 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 mechanism such as a motor, a power source such as a battery, a transmission, tires, etc. for running and operating the vehicle 20. The sensor 40 detects the position, attitude, speed, acceleration, etc. of the vehicle 20, and includes, for example, a gyro sensor, a speed sensor, a camera, etc.

[0023] 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).

[0024] 3, the driving control device 30 is a device provided inside the vehicle 20. However, the driving control device 30 is not limited to being provided inside the vehicle 20, and may be provided outside the vehicle 20 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, or the like, and transmits instructions necessary for controlling the driving control device 30. The vehicle 20 is, for example, an AMR (Autonomous Mobile Robot), an AGV (Automatic Guided Vehicle), or the like.

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

[0027] The execution control unit 100 executes the initial processing unit 102, and then repeatedly executes the acquisition unit 104, the motion planning unit 106, and the wheel driving unit 108 as the vehicle 20 travels. Note that, hereinafter, the vehicle 20 may also be simply referred to as a vehicle. The vehicle's travel mechanism may be a two-wheel differential mechanism, an automobile mechanism (steering mechanism), or an omnidirectional movement mechanism.

[0028] The initial processing unit 102 acquires a target route that is set so as not to interfere with obstacles while the vehicle is traveling. The target route is given as a sequence of positions, for example, (Px1, Py1), (Px2, Py2), (Pxn, Pyn).

[0029] The acquisition unit 104 acquires vehicle state information including the vehicle position, attitude, translational velocity, and angular velocity, as well as location information of obstacles around the vehicle. The vehicle state and obstacle information are detected by the sensor 40. The vehicle state is given, for example, by the vehicle position (x, y) and attitude θ (angle). The translational velocity is given by v and angular velocity ω. Obstacles are given as a sequence of positions on a map, for example, (Ox1, Oy1), (Ox2, Oy2), (Oxn, Oyn).

[0030] The motion planning unit 106 sets candidate states and determines the translational velocity and angular velocity of the vehicle. Note that the motion planning unit 106 may determine at least one of the translational velocity and angular velocity.

[0031] The wheel driving unit 108 drives the wheels of the vehicle based on the information determined by the motion planning unit 106 .

[0032] 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 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 decelerates before the goal, or the maximum speed specified in the vehicle specifications.

[0033] The processing of the motion planning unit 106A includes (1) setting processing and (2) calculation processing. The calculation processing is a cost calculation processing and a calculation processing of adjusted velocities (adjusted translational velocity and adjusted angular velocity).

[0034] (1) Setting Process The motion planning unit 106A performs a process of setting a combination of a plurality of possible translational velocities and possible angular velocities that are discretely prepared.

[0035] Here, an example of the mode of the candidate state set by the motion planning unit 106A will be described. FIG. 4 shows an example of a grid representing the candidate state to be set. A near the center is the current translational velocity Vc and current angular velocity ωc indicating the acquired host vehicle speed. a is the maximum value of acceleration, and α is the maximum value of angular acceleration. The motion planning unit 106A 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 (the time dt time ahead from the current time) based on the acceleration and angular acceleration with the host vehicle speed as the center. When the upper or lower limits of v and ω are reached, the motion planning unit 106A cuts off the range of the candidate state 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 motion planning unit 106A 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.

[0036] (2) The calculation process will be described. As a preprocessing step, the motion planning unit 106A sets a candidate state, which is a sequence of vehicle positions within the prediction target time range, for each set combination based on the acquired positions and postures. Note that, if necessary for calculating the obstacle term (described later), a sequence of postures may be set as the candidate state in addition to the sequence of positions. Next, the motion planning unit 106A calculates a cost including a predetermined error using the set candidate states. The errors included in the cost are the error between the vehicle position at the end of the candidate state and the target path, and the error between the possible translational velocity and the target translational velocity corresponding to the acquired position. Furthermore, the motion planning unit 106A determines, based on the placement information, a combination of adjusted translational velocity and adjusted angular velocity that minimizes the cost, provided that the vehicle and safety area at all positions in the candidate state do not interfere with obstacles.

[0037] The motion planning unit 106A calculates the cost using a cost function, which is expressed by the following equation (1). cost=α*path+β*vel+γ*obstancle ···(1)

[0038] 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 for explaining an example of setting candidate states. The motion planning unit 106A calculates a predicted trajectory from each candidate state 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 motion planning unit 106A sets candidate states, which are a series of vehicle positions within the prediction target time range.

[0039] The motion planning unit 106A performs the following first to fifth calculation processes as a cost calculation process. As a first calculation process, the motion planning unit 106A calculates a following term. A perpendicular line between the end point of the predicted trajectory and the path is found, and the distance is set as the value of the following term. The following term is a term that indicates whether the vehicle will be in a position close to the path after tn time. As a second calculation process, the motion planning unit 106A 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 vehicle position acquired by the acquisition unit 104, among the target translational velocities at each position on the target path acquired by the initial processing unit 102. As a third calculation process, the motion planning unit 106A 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 motion planning unit 106A calculates the cost based on the first to third calculation processes. As a fifth process, the motion planning unit 106A performs the above calculation for all velocity vectors (v, ω), and outputs the combination with the lowest cost among the calculated combinations as the combination of adjusted translational velocities and adjusted angular velocities that results in the minimum cost.

[0040] The calculation of the obstacle term will now be described. Figure 6 is a diagram for explaining the calculation of the obstacle term. Figure 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 state at each time, S indicates the safety area at each time, and each circle 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 time. The second process determines whether the shape of the vehicle in the candidate state for the position and orientation at each time is interfering with the obstacle. In the second process, when the safety area is taken 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, the vehicle in the candidate state, and the safety area. 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 closest point to the obstacle is calculated at each time, 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), a calculation may be performed in which a cost function is based on only the following term and the velocity term without using an obstacle term, and a velocity vector (v, ω) that causes the predicted trajectory to interfere with an obstacle is not adopted as a combination of the adjusted translational velocity and the adjusted angular velocity.

[0041] The wheel driving unit 108A drives the wheels of the vehicle based on the adjusted translational velocity and adjusted angular velocity determined by the motion planning unit 106A. The execution control unit 100A repeatedly executes the acquisition unit 104A, the motion planning unit 106A, and the wheel driving unit 108A as the vehicle 20 travels.

[0042] (Processing flow) Fig. 8 is a flowchart showing the flow of the driving control process by the driving control device 30A according to the first embodiment. When the vehicle 20 is powered on, the CPU 32 reads out the driving control program from the storage device 36, loads it into the memory 34, and executes it, whereby the CPU 32 functions as each functional component of the driving control device 30, and the driving control process shown in Fig. 8 is executed. In executing the driving control program, first, the execution control unit 100A instructs the initial processing unit 102A to start up.

[0043] In step S10, the initial processing unit 102A acquires a target path and a target translational velocity at each position on the target path.

[0044] In step S12, the acquisition unit 104A acquires the vehicle state including the position, attitude, translational velocity, and angular velocity of the vehicle, as well as the location information of obstacles around the vehicle.

[0045] In step S14, the motion planning unit 106A sets candidate states and determines the combination of adjusted translational velocity and adjusted angular velocity that minimizes the cost.

[0046] In step S16, the wheel driving unit 108A drives the wheels of the vehicle based on the adjusted translational velocity and adjusted angular velocity determined by the motion planning unit 106A.

[0047] In step S18, the execution control unit 100A determines whether or not a termination condition is satisfied. If the termination condition is satisfied, the process is terminated. If the termination condition is not satisfied, the process returns to step S12 and is repeated. The termination condition may be set as appropriate, such as when the vehicle has finished traveling, when the vehicle has reached a target point on the target route, or when an instruction to stop the vehicle has been received from the control device 10.

[0048] Next, a subroutine of the operation planning process executed by the operation planning unit 106A in step S 14 will be described. Fig. 9 is a flowchart showing the flow of the operation planning process according to the first embodiment.

[0049] In step S30, the motion planning unit 106A sets a plurality of discretely prepared combinations of possible translational velocities and possible angular velocities.

[0050] In step S32, the operation planning unit 106A sets, for each set combination, a candidate state that is a sequence of vehicle positions within the prediction target time range based on the acquired position and orientation.

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

[0052] In step S36, the combination of adjusted translational velocity and adjusted angular velocity that minimizes the cost is output.

[0053] As described above, the driving control device of this embodiment sets a longer detection distance as the driving speed increases, and in a vehicle that decelerates when an obstacle is detected within that detection distance, it is possible to reduce the frequency of occurrences of the vehicle suddenly decelerating when an obstacle enters the detection distance, allowing the vehicle to drive.

[0054] [Second embodiment] A second embodiment will be described below. The processing of the motion planning unit 106B includes (1) processing for acquiring a target position sequence, (2) processing for calculating a candidate state, and (3) processing for calculating an adjustment speed.

[0055] (1) A process of acquiring a target position sequence will be described. The motion planning unit 106B acquires a target position sequence, which is a sequence of positions generated based on a target route. The target position sequence is x ref , y ref The target attitude is θ ref Let's say.

[0056] (2) The process of calculating the candidate states will be described. The motion planning unit 106B sets a command sequence based on the acquired vehicle state, and calculates candidate states, which are a sequence of candidate vehicle positions, based on the command sequence. The command sequence is a sequence of candidate translational velocities and candidate angular velocities over a target time range for prediction.

[0057] The candidate translational velocity and candidate angular velocity series are calculated, for example, as follows: The candidate translational velocity v is calculated using the formula: current translational velocity + acceleration × dt (predicted time t1, t2, tn). The candidate angular velocity ω is calculated using the formula: θ ref (k+i)-θ ref It is calculated using a formula such as (k+i-1) / dt.

[0058] (3) The process of calculating the adjusted speed will be described. The motion planning unit 106B calculates the adjusted translational speed and adjusted angular speed based on the command sequence that minimizes a predetermined evaluation value. The evaluation value is determined based on the placement information, with the condition that the vehicle and safety area at all positions in the candidate state do not interfere with obstacles. The evaluation value also includes the amount of error between the target position sequence and the candidate state 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.

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

[0060] The objective function is expressed by the following equation (2).

number

number

number

[0061] Note that Np is the prediction horizon, Nc is the control horizon, and Nc <Np、P∈R 3×3 , Q∈R 3×3 , R∈R 3×3 is.

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

number

number

number

number

[0063] Figure 10 shows an example of how the shape of a vehicle can be expressed as a circle. The advantage of using a circumscribing circle, as in 10A, is that it reduces the calculation load. The advantage of using multiple circles, as in 10B, is that it reduces waste relative to the shape of the vehicle.

[0064] Figure 11 shows an image of the candidate state 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 r , y r The locus of the reference path is shown by

[0065] Figure 12 is an image diagram of obstacle detection. In the detection, a circumscribing circle is set for the shape of the vehicle in the candidate state. In the calculation of the constraint in equation (3-4), the distance between the circumscribing circle of each position in the candidate state 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 constraints are calculated to prevent interference with obstacles, and are reflected in the calculation of the objective function.

[0066] Figure 13 is an image diagram of obstacle detection that takes the safety area into consideration. Multiple circles are set for the safety area of ​​the candidate state 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 of the candidate state, 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.

[0067] The wheel driving unit 108B drives the wheels of the vehicle based on the adjusted translational velocity and adjusted angular velocity determined by the action planning unit 106B. The execution control unit 100B repeatedly executes the acquisition unit 104B, the action planning unit 106B, and the wheel driving unit 108B as the vehicle 20 travels.

[0068] (Processing flow) Fig. 14 is a flowchart showing the flow of the driving control process by the driving control device 30B according to the second embodiment. When the vehicle 20 is powered on, the CPU 32 reads out the driving control program from the storage device 36, loads it into the memory 34, and executes it, whereby the CPU 32 functions as each functional component of the driving control device 30, and the driving control process shown in Fig. 14 is executed. In executing the driving control program, first, the execution control unit 100B instructs the initial processing unit 102B to start up.

[0069] In step S110, the initial processing unit 102B acquires a target route.

[0070] In step S112, the acquisition unit 104B acquires the vehicle state including the position, attitude, translational velocity, and angular velocity of the vehicle, as well as the location information of obstacles around the vehicle.

[0071] In step S114, the motion planning unit 106B calculates the adjusted translational velocity and adjusted angular velocity based on the command sequence that minimizes the evaluation value.

[0072] In step S116, the wheel driving unit 108B drives the wheels of the vehicle based on the adjusted translational velocity and adjusted angular velocity determined by the motion planning unit 106B.

[0073] In step S118, the execution control unit 100B determines whether or not a termination condition is satisfied. If the termination condition is satisfied, the process is terminated. If the termination condition is not satisfied, the process returns to step S112 and is repeated. The termination condition may be set as appropriate, such as when the vehicle has finished traveling, when the vehicle has reached a target point on the target route, or when an instruction to stop the vehicle has been received from the control device 10.

[0074] Next, a subroutine of the operation planning process executed by the operation planning unit 106B in step S 116 will be described. Fig. 15 is a flowchart showing the flow of the operation planning process according to the second embodiment.

[0075] In step S130, the motion planning unit 106B acquires a target position sequence, which is a sequence of positions generated based on the target route.

[0076] In step S132, the operation planning unit 106B sets a command sequence based on the acquired vehicle state, and obtains candidate states, which are sequences of candidate vehicle positions, based on the command sequence.

[0077] In step S134, the motion planning unit 106B uses the objective function of the above equation (2) and the constraints of the equations (3-1) to (3-4) to find the adjusted translational velocity and adjusted angular velocity based on the command sequence that minimizes the evaluation value.

[0078] As described above, the driving control device of this embodiment sets a longer detection distance as the driving speed increases, and in a vehicle that decelerates when an obstacle is detected within that detection distance, it is possible to reduce the frequency of occurrences of the vehicle suddenly decelerating when an obstacle enters the detection distance, allowing the vehicle to drive.

[0079] (Method of expressing the shape of a vehicle using multiple circles) A specific method for representing a vehicle shape with multiple circles will now be described. Figures 16 and 17 are diagrams for explaining a method for representing a vehicle shape with multiple circles. Multiple circles are set according to steps (A1) to (A5). In step (A1), a square is calculated whose side length is the same as the shorter side of the vehicle shape 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 end, 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 end. 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 + 1. Based on this value, the positions of the squares are adjusted so that they are evenly spaced. In step (A5), the outer circle of each square is calculated. As described above, a circumscribing circle is set for each of the multiple squares arranged along the length of the vehicle shape. When controlling the attitude based on multiple circles, the center coordinates of each circumscribing circle are rotated according to the attitude of the vehicle.

[0080] [Variation 1] In Modification 1, an embodiment will be described 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 route.

[0081] The motion planning unit 106A performs (1) a process of setting multiple discretely prepared possible translational velocities, and (2) a process of setting candidate states. The possible translational velocities are constant values ​​throughout the prediction target time range. The candidate states are a series of vehicle positions on the target route within the prediction target time range based on the acquired positions and target route for each possible translational speed. Next, the motion planning unit 106A performs (3) a process of calculating an adjusted translational speed. The adjusted translational speed is calculated based on the placement information so that the difference between the possible translational velocities and the target translational speed corresponding to the acquired position is minimized, on the condition that the vehicle and safety area at all positions in the candidate states do not interfere with obstacles. The motion planning unit 106A performs (4) a process of calculating an adjusted angular velocity based on the adjusted translational speed and the target route.

[0082] FIG. 18 is a diagram for explaining a setting mode of the candidate state in the 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 velocities 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 velocities within that range are divided at specific sampling intervals. For each divided possible translational velocity, a predicted trajectory for a time tn that follows the target route is calculated, assuming a constant velocity. The cost of the DWA cost function is calculated, and v (adjusted translational velocity) with the lowest cost is obtained, taking into account the safety area. The angular velocity ω (adjusted angular velocity) is determined so that the vehicle will be positioned on the target route when traveling for dt time at the obtained adjusted translational velocity.

[0083] The processing flow of Modification 1 can be executed by replacing the flowchart of FIG. 9 of the first embodiment. In step S30, the motion planning unit 106A sets several discretely prepared possible translational velocities. In step S32, the motion planning unit 106A sets, for each possible translational velocity, a candidate state that is a sequence of vehicle positions on the target route within the prediction target time range based on the acquired position and target route. In step S34, the motion planning unit 106A calculates the cost using the cost function of equation (1) above, and determines an adjusted translational velocity so that the difference from a predetermined target translational velocity is minimized. In step S36, the motion planning unit 106A determines an adjusted angular velocity based on the adjusted translational velocity and the target route, and outputs the combination.

[0084] [Variation 2] Modification 2 is a mode in which 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 route. The case where this is applied to the DWA of the first embodiment and the case where this is applied to the MPC of the second embodiment will be described.

[0085] Figure 19 is an illustration of how v and ω are calculated using DWA. In the example shown in Figure 19, v and ω are selected to minimize the error between the intersection PA with the target path, 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 Figure 19, or a circle. The position of the intersection PA should be set to a position much farther away than the position that can be reached after tn seconds. In this case, the vehicle will travel toward that distant position, allowing it to 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). The translational speed v in this case is selected to be the maximum translational speed v within the range of possible translational speeds that does not interfere with the safety area.

[0086] Figure 20 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 certain distance from the vehicle and the end of the prediction horizon, uses only the first term; the second and third terms are not required. This is because the position and orientation of points along the trajectory can be uniquely calculated due to kinematic inequality constraints.

[0087] 21 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 obstacles.

[0088] In 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 programmable logic devices (PLDs) whose circuit configuration can be changed after fabrication, such as field-programmable gate arrays (FPGAs), and dedicated electrical circuits, such as application-specific integrated circuits (ASICs), which are processors with circuit configurations specifically designed to perform specific processing. The information processing may be performed by one of these 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). The hardware structure of these processors is, more specifically, an electrical circuit that combines circuit elements such as semiconductor devices.

[0089] 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.

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

[0091] (Additional note 1) A travel control 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 is approximately the same as the width of the vehicle, an initial processing unit (102) that acquires a target route set so that the vehicle body does not interfere with obstacles while traveling; an acquisition unit (104) that acquires vehicle states including a position, an attitude, a translational velocity, and an angular velocity of the vehicle, as well as information on the location of the obstacles around the vehicle; a motion planning unit (106) that sets candidate states for at least a part of the future vehicle states based on the target route, and determines at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area, Cruise control device (30).

[0092] (Additional note 2) The initial processing unit (102) further acquires or sets a target translational velocity at each position on the target path; the motion planning unit (106) performs the following processes: setting a plurality of combinations of possible translational velocities and possible angular velocities, each of which is prepared discretely and has a constant value over a prediction target time range; setting, for each combination, a candidate state that is a series of positions of the vehicle within the prediction target time range based on the position and attitude acquired by the acquisition unit; calculating a cost including an error between the position of the vehicle at an end of the candidate state and the target route, and an error between the possible translational velocities and the target translational velocities corresponding to the positions acquired by the acquisition unit; and determining, based on the placement information, a combination of adjusted translational velocities and adjusted angular velocities that minimizes the cost on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state; a wheel drive unit (108) that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit (100) that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; 2. The travel control device (30) of claim 1.

[0093] (Additional note 3) The initial processing unit (102) further acquires or sets a target translational velocity at each position on the target path; the motion planning unit (106) performs the following processes: setting a plurality of discretely prepared possible translational velocities, each of which has a constant value over a prediction target time range; setting, for each possible translational speed, a candidate state that is a series of positions of the vehicle on the target route within the prediction target time range based on the position acquired by the acquisition unit and the target route; determining, based on the placement information, an adjusted translational speed that minimizes a difference between the possible translational speeds and a target translational speed corresponding to the position acquired by the acquisition unit on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state; and determining an adjusted angular speed based on the adjusted translational speed and the target route; a wheel drive unit (108) that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit (100) that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; 2. The travel control device (30) of claim 1.

[0094] (Additional note 4) the operation planning unit (106) performs a process of acquiring a target position series, which is a series of positions generated based on the target route; a process of setting a command series, which is a series of candidate translational velocities and candidate angular velocities, over a prediction target time range based on the acquired vehicle state, and determining a candidate state, which is a series of candidate positions of the vehicle, based on the command series; and a process of determining an adjusted translational velocities and adjusted angular velocities based on the command series that minimizes an evaluation value including an error amount between the target position series and the candidate state over the prediction target time range, on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state, based on the placement information; a wheel drive unit (108) that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit (100) that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; 2. The travel control device (30) of claim 1.

[0095] (Additional note 5) The travel control device (30) according to any one of Supplementary Note 1 to Supplementary Note 4, 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.

[0096] (Additional note 6) 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, in the travel control device (30) described in Appendix 5.

[0097] (Additional note 7) A vehicle travel control method in which a safety region is set having a length that increases as the translational speed of the vehicle increases from a front end of the vehicle forward and a width that is approximately the same as the width of the vehicle, acquiring a target route set so that the vehicle body does not interfere with obstacles while traveling; Acquire vehicle status information including the position, attitude, translational velocity, and angular velocity of the vehicle, as well as location information of the obstacles around the vehicle; setting candidate states for at least a portion of the future vehicle states based on the target route, and determining at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area; A driving control method in which processing is performed by a computer.

[0098] (Additional note 8) Computer, In a vehicle travel control device (30) for a vehicle, 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 is approximately the same as the width of the vehicle, an initial processing unit (102) for acquiring a target route set so that the vehicle body does not interfere with obstacles while traveling; an acquisition unit (104) that acquires vehicle states including the position, attitude, translational velocity, and angular velocity of the vehicle, as well as location information of the obstacles around the vehicle; and a motion planning unit (106) that sets candidate states for at least a part of the future vehicle states based on the target route, and determines at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area; A driving control program that functions as a [Explanation of symbols]

[0099] 30 Driving control device 100 Execution control unit 102 Initial processing section 104 Acquisition Department 106 Motion Planning Unit 108 Wheel drive unit

Claims

1. A travel control 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 a front end of the vehicle toward the front and a width that is approximately the same as the width of the vehicle, an initial processing unit that acquires a target route set so that the vehicle body does not interfere with obstacles while traveling; an acquisition unit that acquires vehicle status information including a position, an attitude, a translational velocity, and an angular velocity of the vehicle, as well as location information of the obstacles around the vehicle; a motion planning unit that sets candidate states for at least a part of the future vehicle states based on the target route, and determines at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area, Driving control device.

2. The initial processing unit further acquires or sets a target translational velocity at each position on the target path; the operation planning unit performs the following processes: a process of setting a plurality of combinations of possible translational velocities and possible angular velocities, each of which is prepared discretely and has a constant value over a prediction target time range; a process of setting, for each combination, a candidate state which is a series of positions of the vehicle within the prediction target time range based on the position and attitude acquired by the acquisition unit, and a process of calculating a cost including an error between the position of the vehicle at an end of the candidate state and the target route and an error between the possible translational velocities and the target translational velocities corresponding to the positions acquired by the acquisition unit; and a process of finding, based on the placement information, a combination of adjusted translational velocities and adjusted angular velocities which minimizes the cost on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state; a wheel drive unit that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; The cruise control device according to claim 1.

3. The initial processing unit further acquires or sets a target translational velocity at each position on the target path; the operation planning unit performs the following processes: setting a plurality of discretely prepared possible translational velocities, each of which has a constant value over a prediction target time range; setting, for each possible translational speed, a candidate state that is a series of positions of the vehicle on the target route within the prediction target time range based on the position acquired by the acquisition unit and the target route; calculating, based on the placement information, an adjusted translational speed that minimizes a difference between the possible translational velocities and a target translational speed corresponding to the position acquired by the acquisition unit on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state; and calculating an adjusted angular speed based on the adjusted translational speed and the target route. a wheel drive unit that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; The cruise control device according to claim 1.

4. the operation planning unit performs a process of acquiring a target position series, which is a series of positions generated based on the target route; a process of setting a command series, which is a series of candidate translational velocities and candidate angular velocities, over a prediction target time range based on the acquired vehicle state, and determining a candidate state, which is a series of candidate positions of the vehicle, based on the command series; and a process of determining an adjusted translational velocities and adjusted angular velocities based on the command series that minimizes an evaluation value including an error amount between the target position series and the candidate state over the prediction target time range, on the condition that the vehicle and the safety area do not interfere with the obstacle at all positions in the candidate state, based on the placement information; a wheel drive unit that drives wheels of the vehicle based on the adjusted translational velocity and the adjusted angular velocity; an execution control unit that executes the initial processing unit, and then repeatedly executes the acquisition unit, the operation planning unit, and the wheel driving unit as the vehicle travels; The cruise control device according to claim 1.

5. The cruise control device according to claim 1 , 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.

6. 6. The driving control device according to claim 5, 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.

7. A vehicle travel control method in which a safety region is set having a length that increases as the translational speed of the vehicle increases from a front end of the vehicle forward and a width that is approximately the same as the width of the vehicle, acquiring a target route set so that the vehicle body does not interfere with obstacles while traveling; Acquire vehicle status information including the position, attitude, translational velocity, and angular velocity of the vehicle, as well as location information of the obstacles around the vehicle; setting candidate states for at least a portion of the future vehicle states based on the target route, and determining at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area; A driving control method in which processing is performed by a computer.

8. Computer, In a vehicle travel control device, 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 is approximately the same as the width of the vehicle, an initial processing unit that acquires a target route set so that the vehicle body does not interfere with obstacles while traveling; an acquisition unit that acquires vehicle status information including a position, an attitude, a translational velocity, and an angular velocity of the vehicle, as well as location information of the obstacles around the vehicle; and a motion planning unit that sets candidate states for at least a part of the future vehicle states based on the target route, and determines at least one of a translational velocity and an angular velocity of the vehicle based on the candidate states so that the obstacle does not enter the safety area; A driving control program that functions as a

Images