JP7699435B2 - Vehicle control device, vehicle control method, and program - Google Patents

Description

The present invention relates to a vehicle control device, a vehicle control method, and a program.

Conventionally, there is known a technique for applying a trajectory model based on an arc shape to a target trajectory of a vehicle to control the steering angle. For example, Patent Document 1 discloses a technique for creating a trajectory model by arc fitting using a gradient descent method.

JP 2018-203018 A

However, because the technology described in Patent Document 1 uses gradient descent, it may take a long time to calculate until the solution converges. Furthermore, because the technology described in Patent Document 1 does not allow the number of vehicle trajectory points to which the trajectory model is applied to be variable, it may not be possible to create a flexible trajectory model that corresponds to the target trajectory of the vehicle.

The present invention has been made in consideration of these circumstances, and one of its objectives is to provide a vehicle control device, a vehicle control method, and a program that can more quickly and flexibly create a trajectory model that corresponds to the target trajectory of the vehicle.

The vehicle control device according to the present invention employs the following configuration.
(1): A vehicle control device according to one embodiment of the present invention includes an acquisition unit that acquires a plurality of trajectory points constituting a target trajectory that a vehicle will pass in the future, a trajectory point setting unit that sets a start point and an end point to which a trajectory model is to be applied from the plurality of trajectory points, and a trajectory model calculation unit that calculates a trajectory model applied to target trajectory points between the start point and the end point among the plurality of trajectory points, and calculates an error between the trajectory model and the target trajectory points, and causes the vehicle to run along the determined trajectory model, and the trajectory point setting unit updates the start point and the end point based on the error.

(2): In the aspect of (1) above, when the error is greater than the upper error limit or less than the lower error limit, the trajectory point setting unit updates the end point based on the minimum or maximum allowable value of the end point, and updates the start point by multiplying the updated end point by a predetermined value, thereby repeating the process of updating the start point.

(3): In the above aspect (1) or (2), the trajectory point setting unit calculates the error by the squared error between the trajectory model and the target trajectory point.

(4): In the aspect of (3) above, the trajectory model is a circular arc, and the trajectory model calculation unit calculates a reference point of the vehicle based on the radius of the arc and the wheelbase of the vehicle, and calculates the squared error by calculating the square of the difference between the square of the distance between the reference point and the center of the arc and the square of the distance between the reference point and the target trajectory point.

(5): In another aspect of the present invention, a vehicle control method includes a vehicle control device that acquires a plurality of trajectory points constituting a target trajectory along which the vehicle will pass in the future, sets a start point and an end point to which a trajectory model is to be applied from the plurality of trajectory points, calculates a trajectory model applied to target trajectory points between the start point and the end point among the plurality of trajectory points, calculates an error between the trajectory model and the target trajectory points, drives the vehicle along the determined trajectory model, and updates the start point and the end point based on the error.

(6): A program according to another aspect of the present invention causes a processor of a vehicle control device to acquire a plurality of trajectory points constituting a target trajectory along which the vehicle will pass in the future, set a start point and an end point to which a trajectory model is to be applied from the plurality of trajectory points, calculate a trajectory model applied to target trajectory points between the start point and the end point among the plurality of trajectory points, calculate an error between the trajectory model and the target trajectory points, run the vehicle along the determined trajectory model, and update the start point and the end point based on the error.

By using (1) to (6), a trajectory model that corresponds to the target trajectory of the vehicle can be created more quickly and flexibly.

1 is a configuration diagram of a vehicle system 1 that uses a vehicle control device according to an embodiment. FIG. 2 is a functional configuration diagram of a first control unit 120 and a second control unit 160. FIG. 13 is a diagram showing an example of a scene in which target trajectory points are extracted by a trajectory point setting unit 166A. FIG. 13 is a diagram showing another example of a scene in which target trajectory points are extracted by the trajectory point setting unit 166A. FIG. 13 is a diagram showing another example of a scene in which target trajectory points are extracted by the trajectory point setting unit 166A. FIG. 13 is a diagram showing an extraction algorithm of target trajectory points by a trajectory point setting unit 166A. FIG. 13 is a diagram showing a mechanism for calculating an orbit model by an orbit model calculation unit 166B. 13 is a flowchart showing an example of a flow of processing executed by a trajectory point setting unit 166A and a trajectory model calculation unit 166B in cooperation with each other.

The following describes a vehicle control device, a vehicle control method, and a program according to an embodiment of the present invention with reference to the drawings.

[Overall configuration]
1 is a configuration diagram of a vehicle system 1 that uses a vehicle control device according to an embodiment. The vehicle on which the vehicle system 1 is mounted is, for example, a two-wheeled, three-wheeled, or four-wheeled vehicle, and its drive source is an internal combustion engine such as a diesel engine or a gasoline engine, an electric motor, or a combination of these. The electric motor operates using power generated by a generator connected to the internal combustion engine, or discharged power from a secondary battery or a fuel cell.

The vehicle system 1 includes, for example, a camera 10, a radar device 12, a LIDAR (Light Detection and Ranging) 14, an object recognition device 16, a communication device 20, an HMI (Human Machine Interface) 30, a vehicle sensor 40, a navigation device 50, an MPU (Map Positioning Unit) 60, a driving operator 80, an automatic driving control device 100, a driving force output device 200, a brake device 210, and a steering device 220. These devices and equipment are connected to each other by multiple communication lines such as a CAN (Controller Area Network) communication line, serial communication lines, wireless communication networks, etc. Note that the configuration shown in FIG. 1 is merely an example, and some of the configuration may be omitted, or other configurations may be added.

The camera 10 is, for example, a digital camera that uses a solid-state imaging element such as a CCD (Charge Coupled Device) or a CMOS (Complementary Metal Oxide Semiconductor). The camera 10 is attached to any location of the vehicle (hereinafter, the vehicle M) in which the vehicle system 1 is mounted. When capturing an image of the front, the camera 10 is attached to the top of the front windshield, the back of the rearview mirror, or the like. The camera 10, for example, periodically and repeatedly captures images of the surroundings of the vehicle M. The camera 10 may be a stereo camera.

The radar device 12 emits radio waves such as millimeter waves around the vehicle M and detects radio waves reflected by objects (reflected waves) to detect at least the position (distance and direction) of the object. The radar device 12 is attached to any location on the vehicle M. The radar device 12 may detect the position and speed of an object by using the FM-CW (Frequency Modulated Continuous Wave) method.

The LIDAR 14 irradiates light (or electromagnetic waves with a wavelength close to that of light) around the vehicle M and measures the scattered light. The LIDAR 14 detects the distance to the target based on the time between emitting and receiving the light. The irradiated light is, for example, a pulsed laser light. The LIDAR 14 can be attached to any location on the vehicle M.

The object recognition device 16 performs sensor fusion processing on the detection results from some or all of the camera 10, the radar device 12, and the LIDAR 14 to recognize the position, type, speed, etc. of the object. The object recognition device 16 outputs the recognition results to the autonomous driving control device 100. The object recognition device 16 may output the detection results from the camera 10, the radar device 12, and the LIDAR 14 directly to the autonomous driving control device 100. The object recognition device 16 may be omitted from the vehicle system 1.

The communication device 20 communicates with other vehicles in the vicinity of the vehicle M, for example, using a cellular network, a Wi-Fi network, Bluetooth (registered trademark), or DSRC (Dedicated Short Range Communication), or communicates with various server devices via a wireless base station.

The HMI 30 presents various information to the occupants of the vehicle M and accepts input operations by the occupants. The HMI 30 includes various display devices, speakers, buzzers, touch panels, switches, keys, etc.

The vehicle sensor 40 includes a vehicle speed sensor that detects the speed of the host vehicle M, an acceleration sensor that detects the acceleration, a yaw rate sensor that detects the angular velocity around the vertical axis, and a direction sensor that detects the direction of the host vehicle M.

The navigation device 50 includes, for example, a GNSS (Global Navigation Satellite System) receiver 51, a navigation HMI 52, and a route determination unit 53. The navigation device 50 holds first map information 54 in a storage device such as a HDD (Hard Disk Drive) or a flash memory. The GNSS receiver 51 identifies the position of the vehicle M based on a signal received from a GNSS satellite. The position of the vehicle M may be identified or supplemented by an INS (Inertial Navigation System) using the output of the vehicle sensor 40. The navigation HMI 52 includes a display device, a speaker, a touch panel, a key, and the like. The navigation HMI 52 may be partially or entirely shared with the above-mentioned HMI 30. The route determination unit 53 determines, for example, a route (hereinafter, a route on a map) from the position of the vehicle M identified by the GNSS receiver 51 (or any input position) to a destination input by the occupant using the navigation HMI 52, by referring to the first map information 54. The first map information 54 is, for example, information that expresses road shapes using links that indicate roads and nodes connected by the links. The first map information 54 may also include road curvature and POI (Point Of Interest) information. The route on the map is output to the MPU 60. The navigation device 50 may perform route guidance using the navigation HMI 52 based on the route on the map. The navigation device 50 may be realized by the functions of a terminal device such as a smartphone or tablet terminal owned by the occupant. The navigation device 50 may transmit the current position and the destination to a navigation server via the communication device 20 and obtain a route equivalent to the route on the map from the navigation server.

The MPU 60 includes, for example, a recommended lane determination unit 61, and stores second map information 62 in a storage device such as an HDD or flash memory. The recommended lane determination unit 61 divides the route on the map provided by the navigation device 50 into a number of blocks (for example, every 100 m in the vehicle travel direction), and determines a recommended lane for each block by referring to the second map information 62. The recommended lane determination unit 61 determines, for example, which lane from the left to use. When a branch point is present on the route on the map, the recommended lane determination unit 61 determines a recommended lane so that the vehicle M can use a reasonable route to proceed to the branch point.

The second map information 62 is map information with higher accuracy than the first map information 54. The second map information 62 includes, for example, information on the center of lanes or information on lane boundaries. The second map information 62 may also include road information, traffic regulation information, address information (address and zip code), facility information, telephone number information, and the like. The second map information 62 may be updated at any time by the communication device 20 communicating with other devices.

The driving operators 80 include, for example, an accelerator pedal, a brake pedal, a shift lever, a steering wheel, a special steering wheel, a joystick, and other operators. The driving operators 80 are fitted with sensors that detect the amount of operation or the presence or absence of operation, and the detection results are output to the automatic driving control device 100, or some or all of the driving force output device 200, the brake device 210, and the steering device 220.

The automatic driving control device 100 includes, for example, a first control unit 120 and a second control unit 160. The first control unit 120 and the second control unit 160 are each realized by, for example, a hardware processor such as a CPU (Central Processing Unit) executing a program (software). In addition, some or all of these components may be realized by hardware (including circuitry) such as an LSI (Large Scale Integration), an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a GPU (Graphics Processing Unit), or may be realized by collaboration between software and hardware. The program may be stored in advance in a storage device (storage device having a non-transient storage medium) such as an HDD or flash memory of the automatic driving control device 100, or may be stored in a removable storage medium such as a DVD or CD-ROM, and the storage medium (non-transient storage medium) may be installed in the HDD or flash memory of the automatic driving control device 100 by attaching the storage medium to the drive device. The automatic driving control device 100 is an example of a "vehicle control device."

FIG. 2 is a functional configuration diagram of the first control unit 120 and the second control unit 160. The first control unit 120 includes, for example, a recognition unit 130 and an action plan generation unit 140. The first control unit 120 realizes, for example, a function based on AI (Artificial Intelligence) and a function based on a pre-given model in parallel. For example, the function of "recognizing an intersection" may be realized by executing in parallel recognition of the intersection using deep learning or the like and recognition based on pre-given conditions (such as traffic lights and road markings that can be pattern matched), and scoring both and evaluating them comprehensively. This ensures the reliability of autonomous driving.

The recognition unit 130 recognizes the position, speed, acceleration, and other states of objects around the vehicle M based on information input from the camera 10, the radar device 12, and the LIDAR 14 via the object recognition device 16. The position of the object is recognized as a position on absolute coordinates with a representative point of the vehicle M (such as the center of gravity or the center of the drive shaft) as the origin, and is used for control. The position of the object may be represented by a representative point such as the center of gravity or a corner of the object, or may be represented by a represented area. The "state" of the object may include the acceleration or jerk of the object, or the "behavioral state" (for example, whether or not the object is changing lanes or is about to change lanes).

The recognition unit 130 also recognizes, for example, the lane in which the vehicle M is traveling (the driving lane). For example, the recognition unit 130 recognizes the driving lane by comparing the pattern of road dividing lines (for example, an arrangement of solid and dashed lines) obtained from the second map information 62 with the pattern of road dividing lines around the vehicle M recognized from the image captured by the camera 10. Note that the recognition unit 130 may recognize the driving lane by recognizing road boundaries (road boundaries) including road dividing lines, shoulders, curbs, medians, guard rails, etc., in addition to road dividing lines. In this recognition, the position of the vehicle M obtained from the navigation device 50 and the processing results by the INS may be taken into account. The recognition unit 130 also recognizes stop lines, obstacles, red lights, toll booths, and other road phenomena.

When recognizing the driving lane, the recognition unit 130 recognizes the position and attitude of the host vehicle M with respect to the driving lane. For example, the recognition unit 130 may recognize the deviation of the reference point of the host vehicle M from the center of the lane and the angle with respect to a line connecting the centers of the lanes in the traveling direction of the host vehicle M as the relative position and attitude of the host vehicle M with respect to the driving lane. Alternatively, the recognition unit 130 may recognize the position of the reference point of the host vehicle M with respect to either side end of the driving lane (a road dividing line or a road boundary) as the relative position of the host vehicle M with respect to the driving lane.

In principle, the action plan generation unit 140 drives the recommended lane determined by the recommended lane determination unit 61, and further generates a target trajectory along which the host vehicle M will automatically (without the driver's operation) drive in the future so that the host vehicle M can respond to the surrounding conditions of the host vehicle M. The target trajectory includes, for example, a speed element. For example, the target trajectory is expressed as a sequence of points (trajectory points) to be reached by the host vehicle M. The trajectory points are points to be reached by the host vehicle M at each predetermined travel distance (for example, about several meters) along the road, and separately, the target speed and target acceleration for each predetermined sampling time (for example, about a few tenths of a second) are generated as part of the target trajectory. The trajectory points may also be positions to be reached by the host vehicle M at each sampling time for each predetermined sampling time. In this case, the information on the target speed and target acceleration is expressed as the interval between the trajectory points.

The behavior plan generation unit 140 may set an autonomous driving event when generating the target trajectory. Autonomous driving events include a constant speed driving event, a low speed following driving event, a lane change event, a branching event, a merging event, and a takeover event. The behavior plan generation unit 140 generates a target trajectory according to the activated event.

The second control unit 160 controls the driving force output device 200, the brake device 210, and the steering device 220 so that the host vehicle M passes through the target trajectory generated by the action plan generation unit 140 at the scheduled time.

Returning to FIG. 2, the second control unit 160 includes, for example, an acquisition unit 162, a speed control unit 164, and a steering control unit 166. The acquisition unit 162 acquires information on the target trajectory (trajectory points) generated by the action plan generation unit 140 and stores it in a memory (not shown). The speed control unit 164 controls the driving force output device 200 or the brake device 210 based on the speed element associated with the target trajectory stored in the memory. The steering control unit 166 controls the steering device 220 according to the curvature of the target trajectory stored in the memory. The processing of the speed control unit 164 and the steering control unit 166 is realized, for example, by a combination of feedforward control and feedback control. As an example, the steering control unit 166 executes a combination of feedforward control according to the curvature of the road ahead of the vehicle M and feedback control based on the deviation from the target trajectory.

The steering control unit 166 further extracts a specific trajectory point (hereinafter referred to as a "target trajectory point") from the multiple trajectory points that constitute the target trajectory, and calculates a trajectory model to be fitted to the target trajectory point. In this embodiment, the steering control unit 166 calculates an arc with a constant curvature as a trajectory model to be fitted to the target trajectory point, but the present invention is not limited to this configuration, and a trajectory model of any shape with a non-constant curvature can also be calculated. If the error between the calculated arc and the target trajectory point is within a predetermined range, the steering control unit 166 controls the steering device 220 so that the host vehicle M travels according to the arc. On the other hand, if the error is not within the predetermined range, the steering control unit 166 re-extracts the target trajectory point and re-calculates the arc. To achieve this function, the steering control unit 166 includes a trajectory point setting unit 166A and a trajectory model calculation unit 166B, but the details of these functional units will be described later.

The driving force output device 200 outputs a driving force (torque) to the drive wheels for the vehicle to travel. The driving force output device 200 includes, for example, a combination of an internal combustion engine, an electric motor, and a transmission, and an ECU (Electronic Control Unit) that controls these. The ECU controls the above configuration according to information input from the second control unit 160 or information input from the driving operator 80.

The brake device 210 includes, for example, a brake caliper, a cylinder that transmits hydraulic pressure to the brake caliper, an electric motor that generates hydraulic pressure in the cylinder, and a brake ECU. The brake ECU controls the electric motor according to information input from the second control unit 160 or information input from the driving operation unit 80, so that a brake torque corresponding to the braking operation is output to each wheel. The brake device 210 may include a backup mechanism that transmits hydraulic pressure generated by operating the brake pedal included in the driving operation unit 80 to the cylinder via a master cylinder. Note that the brake device 210 is not limited to the configuration described above, and may be an electronically controlled hydraulic brake device that controls an actuator according to information input from the second control unit 160 to transmit hydraulic pressure from the master cylinder to the cylinder.

The steering device 220 includes, for example, a steering ECU and an electric motor. The electric motor changes the direction of the steered wheels by, for example, applying a force to a rack and pinion mechanism. The steering ECU drives the electric motor according to information input from the second control unit 160 or information input from the driving operator 80, to change the direction of the steered wheels.

[Extraction of target trajectory points and calculation of trajectory model]
The extraction of target trajectory points by the trajectory point setting unit 166A and the calculation of the trajectory model by the trajectory model calculation unit 166B will be described below.

The trajectory point setting unit 166A extracts target trajectory points to be used for calculating a trajectory model from a plurality of trajectory points constituting the target trajectory. Fig. 3 is a diagram showing an example of a scene in which the target trajectory points are extracted by the trajectory point setting unit 166A. Fig. 3 shows a scene in which the host vehicle M turns right before an intersection, and the coordinates of each point are shown with a point O on the rear wheel axle RL of the host vehicle M as the origin. _P1 ( _x1 , _y1 ) to _P7 ( _x7 , _y7 ) indicate the trajectory points of the host vehicle M, FL indicates the front wheel axle, 1 indicates the wheelbase of the host vehicle M (i.e., the distance between the front wheel axle FL and the rear wheel axle RL), C indicates the trajectory model (arc), RP(1/2, r) indicates the reference point on the host vehicle where the arc C starts, RC indicates the midpoint of the rear wheel, and TP indicates the target trajectory point among _P1 ( _x1 , _y1 ) to _P7 ( _x7 , _y7 ) to which the arc C is fitted (hereinafter, the fitting is also referred to as "fitting"). The reference point RP(1/2, r) is located at the midpoint of the wheelbase of the host vehicle M in the longitudinal direction, and can be said to be located at the midpoint of the vehicle body in the lateral direction of the host vehicle M. For convenience, P _n (x _n , _yn ) (n is an integer) indicates a trajectory point of the host vehicle M n seconds after the current time, but is not limited to this configuration.

In Fig. 3, the trajectory point setting unit 166A extracts _P1 ( _x1 , _y1 ) to _P5 ( _x5 , _y5 ), i.e., five trajectory points, as the target trajectory points TP. Hereinafter, the number of trajectory points extracted as the target trajectory points TP is referred to as the "fitting length". In this case, _P1 ( _x1 , _y1 ) is the start point of the target trajectory points TP, and _P5 ( _x5 , _y5 ) is the end point of the target trajectory points TP. Conversely, by determining the start point and end point of the target trajectory points TP, the trajectory points extracted as the target trajectory points TP and their number are also determined. In other words, extracting the target trajectory points TP and determining the start point and end point of the target trajectory points TP are substantially the same thing.

Fig. 4 is a diagram showing another example of a scene in which the target trajectory points are extracted by the trajectory point setting unit 166A. In Fig. 4, the trajectory point setting unit 166A extracts _P1 ( _x1 , _y1 ) to _P7 ( _x7 , _y7 ) as the target trajectory points TP, that is, target trajectory points TP with a fitting length of 7. However, if the steering control unit 166 steers the vehicle M along the arc C fitted to the target trajectory points TP as shown in Fig. 4, the vehicle M will deviate excessively to the right on the road on which it should go straight after turning right at an intersection and enter an adjacent lane. This means that the fitting length set in Fig. 4 is too long.

Fig. 5 is a diagram showing yet another example of a scene in which the target trajectory points are extracted by the trajectory point setting unit 166A. In Fig. 5, the trajectory point setting unit 166A extracts _P1 ( _x1 , _y1 ) to _P3 ( _x3 , _y3 ) as the target trajectory points TP, that is, target trajectory points TP with a fitting length of 3. However, if the steering control unit 166 steers the host vehicle M along the arc C fitted to the target trajectory points TP as shown in Fig. 5, the steering control will be switched in the middle of turning right along the arc C, and smooth steering will be hindered. This means that the fitting length set in Fig. 5 is too short.

In this way, the trajectory point setting unit 166A is required to extract target trajectory points TP having an appropriate fitting length so that the fitting length is neither too long nor too short. Below, the configuration of the trajectory point setting unit 166A that extracts target trajectory points TP having an appropriate fitting length will be described with reference to FIG. 6.

Fig. 6 is a diagram showing an extraction algorithm of the target trajectory point by the trajectory point setting unit 166A. In Fig. 6, start indicates the start point of the target trajectory point TP, end indicates the end point of the target trajectory point TP, end _max indicates the maximum allowable value that the end point can take, end _min indicates the minimum allowable value that the end point can take, and const indicates a predetermined value greater than 0 and less than 1. Furthermore, err indicates the error between the calculated arc C and the target trajectory point TP, err _max indicates the upper error limit value which is the maximum allowable error value, and err _min indicates the lower error limit value which is the minimum allowable error value. A specific method of calculating the error will be described later.

When the error err is greater than the upper error limit err _max or less than the lower error limit err _min , the trajectory point setting unit 166A updates the end point end based on the minimum allowable value end _min or maximum allowable value err _max of the end point end, and updates the start point start by multiplying the updated end point end by a predetermined value const, thereby updating the start point start and the end point end. When the error err becomes greater than or equal to the lower error limit err _min and less than or equal to the upper error limit err _max , the trajectory point setting unit 166A determines the start point start and end point end at that time as the start point and end point of the target trajectory point TP, and extracts the target trajectory point TP between them.

Specifically, for example, in the first extraction shown in FIG. 6, it is assumed that end _max = 7, end _min = 1, const = 0.3, and end = end _max . In this case, since start = end × const = 7 × 0.3 = 2.1, the decimal point is discarded and start = 2. That is, in the first extraction, the trajectory point setting unit 166A extracts P ₂ (x ₂ , y ₂ ) to P ₇ (x ₇ , y ₇ ) as target trajectory points TP of fitting length 6. In the first extraction, since it is determined that err > err _max , the trajectory point setting unit 166A updates the end point end based on the minimum allowable value end _min . That is, the trajectory point setting unit 166A updates the end point end to (end + end _min )/2 = (7 + 1)/2 = 4. In response to this, the trajectory point setting unit 166A updates the start point start to 1 because the start point start = end x const = 4 x 0.3 = 1.2. As a result, the trajectory point setting unit 166A re-extracts _P1 ( _x1 , _y1 ) to _P4 ( _x4 , _y4 ) as target trajectory points TP of fitting length 4, and the trajectory model calculation unit 166B re-calculates the arc C that fits to the target trajectory point TP (second extraction). However, in the second extraction, it is determined that the error err between the arc C and the target trajectory point TP satisfies err < err _min , so the trajectory point setting unit 166A updates the end point end based on the maximum allowable value end _max . That is, the trajectory point setting unit 166A updates the end point end to 5 because the end point end = (end + end _max )/2 = (4 + 7)/2 = 5.5. In response to this, the trajectory point setting unit 166A updates the start point start to 1 because the start point start = end × const = 5 × 0.3 = 1.5. As a result, the trajectory point setting unit 166A re-extracts P ₁ (x ₁ , y ₁ ) to P ₅ (x ₅ , y ₅ ) as target trajectory points TP of fitting length 5, and the trajectory model calculation unit 166B re-calculates the circular arc C that fits to the target trajectory points TP (third extraction). In the third extraction, since it is determined that the error err between the arc C and the target trajectory point TP satisfies err _min ≦err≦err _max , the trajectory point setting unit 166A finally determines P ₁ (x ₁ , y ₁ ) to P ₅ (x ₅ , y ₅ ) as the target trajectory points TP.

According to the above example, the trajectory point setting unit 166A updates the fitting length from 6 to 4 to 5 based on the error err between the arc C and the target trajectory point TP. In other words, by using the above algorithm, the trajectory point setting unit 166A can extract a target trajectory point TP that has an appropriate fitting length.

Next, the calculation of the trajectory model by the trajectory model calculation unit 166B will be described with reference to FIG. 7. FIG. 7 is a diagram showing the mechanism of the calculation of the trajectory model by the trajectory model calculation unit 166B. The trajectory model calculation unit 166B acquires a plurality of trajectory points constituting the target trajectory from the first control unit 120, and acquires from the trajectory point setting unit 166A the start point and end point that are to become the target trajectory point TP among the plurality of trajectory points. In the first calculation, the start point and end point provided by the trajectory point setting unit 166A may be default values, as shown in the algorithm in FIG. 6.

The trajectory model calculation unit 166B identifies a target trajectory point TP based on the multiple trajectory points acquired from the first control unit 120 and the start point and end point acquired from the trajectory point setting unit 166A. Here, it is assumed that n trajectory points _Pk ( _xk , _yk ) (k=1, 2, ... n) have been identified. Next, the trajectory model calculation unit 166B calculates the radius r of an arc C that fits the identified target trajectory point _Pk ( _xk , _yk ) based on formula (1).

Next, the trajectory model calculation unit 166B calculates an arc C whose center is the origin O, which is the end point of a line segment extending from the midpoint RC of the rear wheels along the rear wheel axle RL by a length of radius r, whose start point is the reference point RP, and whose end point is the end point of a line segment extending from the origin O toward _Pn ( _xn , _yn ) by a length of radius r. Here, the direction in which the line segment is extended from the midpoint RC of the rear wheels along the rear wheel axle RL by a length of radius r to set the origin O can be either left or right, but whether the line segment is extended to the right or left to set the origin O can be determined based on the inclination of the multiple trajectory points obtained from the first control unit 120 (i.e., whether the target trajectory of the host vehicle M is to the right or left).

Next, the trajectory model calculation unit 166B calculates the error err between the calculated arc C and the target trajectory point _Pk ( _xk , _yk ) based on the formula (2).

That is, the trajectory model calculation unit 166B calculates the square error between the arc C and the target trajectory point _Pk ( _xk , _yk ) by calculating the square of the difference between the square of the distance between the reference point RP and the center O of the arc C and the square of the distance between the center O of the arc C and the target trajectory point _Pk ( _xk , _yk ), and defines this as the error err. Next, the trajectory model calculation unit 166B determines whether the calculated error err is equal to or greater than the error lower limit value _errmin and equal to or less than the error upper limit value _errmax . If the calculated error err is equal to or greater than the error lower limit value _errmin and equal to or less than the error upper limit value _errmax , the trajectory model calculation unit 166B outputs a steering angle such that the host vehicle M travels along the calculated arc C, and the steering control unit 166 steers the host vehicle M according to the steering angle. On the other hand, if the calculated error err is smaller than the lower error limit value err _min or larger than the upper error limit value err _max , the trajectory model calculation unit 166B outputs the error err to the trajectory point setting unit 166A, and the trajectory point setting unit 166A updates the start point and the end point of the target trajectory point TP based on the error err, as described above with reference to FIG. 6.

[Operation flow]
Next, a flow of processing executed by the steering control unit 166 will be described with reference to Fig. 8. Fig. 8 is a flowchart showing an example of a flow of processing executed by the trajectory point setting unit 166A and the trajectory model calculation unit 166B in cooperation with each other.

First, when the trajectory point setting unit 166A acquires a plurality of trajectory points constituting the target trajectory from the first control unit 120, it sets a start point and an end point to be fitted into an arc from among the trajectory points (S100). Next, the trajectory point setting unit 166A extracts target trajectory points, which are trajectory points between the set start point and end point, from among the acquired plurality of trajectory points (S101). Next, the trajectory model calculation unit 166B calculates an arc fitted to the extracted target trajectory points (S102). Next, the trajectory model calculation unit 166B determines whether the error between the calculated arc and the target trajectory point is equal to or less than the upper error limit (S103). If the error between the calculated arc and the target trajectory point is equal to or less than the upper error limit, the trajectory model calculation unit 166B determines whether the error is equal to or greater than the lower error limit (S104). If the error is equal to or greater than the lower error limit, the trajectory model calculation unit 166B outputs a steering angle that causes the host vehicle M to travel along the calculated arc C, and ends the process.

On the other hand, in step S103, if the error between the calculated arc and the target trajectory point is greater than the upper error limit, the trajectory point setting unit 166A updates the end point based on the minimum allowable value of the end point (S105). Also, in step S104, if the error between the calculated arc and the target trajectory point is less than the lower error limit, the trajectory point setting unit 166A updates the end point based on the maximum allowable value of the end point (S106). Next, the trajectory point setting unit 166A updates the start point based on the updated end point. Specifically, as described above, the trajectory point setting unit 166A updates the start point by multiplying the updated end point by a predetermined value (S107). After that, the trajectory point setting unit 166A returns the process to S101, and the trajectory model calculation unit 166B extracts the target trajectory point based on the updated start point and end point.

As described above, according to the embodiment of the present invention, the trajectory point setting unit 166A sets the fitting start point, end point, and target trajectory point according to the trajectory points that constitute the acquired target trajectory, and the trajectory model calculation unit 166B calculates the arc and its error by simple calculation based on the set start point, end point, and target trajectory point. Furthermore, if the calculated error is not within a predetermined range, the trajectory point setting unit 166A updates the fitting start point, end point, and target trajectory point so that the calculated error is within the predetermined range, and the trajectory model calculation unit 166B recalculates the arc based on the updated start point, end point, and target trajectory point. This allows the automatic driving control device 100 of the embodiment to more quickly and flexibly create a trajectory model that corresponds to the target trajectory of the vehicle.

The above-described embodiment can be expressed as follows.
A storage device storing a program;
a hardware processor;
The hardware processor executes the program stored in the storage device,
Acquire a plurality of trajectory points constituting a target trajectory along which the vehicle will pass in the future;
From the plurality of trajectory points, a start point and an end point to which a trajectory model is to be fitted are set;
Calculating a trajectory model fitted to a target trajectory point between the start point and the end point among the plurality of trajectory points, and calculating an error between the trajectory model and the target trajectory point;
Running the vehicle along the determined trajectory model;
the trajectory point setting unit updates the start point and the end point based on the error.
The vehicle control device is configured as follows.

The above describes the form for carrying out the present invention using an embodiment, but the present invention is not limited to such an embodiment, and various modifications and substitutions can be made without departing from the spirit of the present invention.

10 Camera 12 Radar device 14 LIDAR
16 Object recognition device 100 Automatic driving control device 120 First control unit 130 Recognition unit 140 Action plan generation unit 160 Second control unit 162 Acquisition unit 164 Speed control unit 166 Steering control unit 166A Trajectory point setting unit 166B Trajectory model calculation unit

Claims (5)

an acquisition unit that acquires a predetermined number of trajectory points constituting a target trajectory that a vehicle will pass in the future, in order of proximity from a reference point of the vehicle;
a trajectory point setting unit that sets a start point and an end point to which a trajectory model, which is a circular arc, is to be fitted from the plurality of trajectory points;
a trajectory model calculation unit that calculates a radius of the arc that fits to the target trajectory point based on a target trajectory point between the start point and the end point among the plurality of trajectory points according to a predefined calculation formula , calculates the center of the arc as a point shifted from the reference point by a predetermined amount based on the radius , and calculates the trajectory model by calculating an error between a distance between the reference point of the vehicle and the center of the arc and a distance between the center of the arc and the target trajectory point,
A vehicle control device that causes the vehicle to travel along the calculated trajectory model,
the trajectory point setting unit sets a trajectory point corresponding to the last order among the orders of the plurality of trajectory points as an initial value of the end point, sets a trajectory point in an order corresponding to a multiplied value obtained by multiplying the number of the plurality of trajectory points by a predetermined value greater than zero and less than one as an initial value of the start point, and updates the start point and the end point based on the error;
the reference point is a point on the vehicle where the arc begins;
the trajectory point setting unit updates the end point as a trajectory point corresponding to an order between the first order and the order of the end point among the plurality of trajectory points when the error is greater than an upper error limit value, and updates the start point as a trajectory point of an order corresponding to a multiplied value obtained by multiplying the updated order of the end point by the predetermined value , while updating the end point as a trajectory point of an order between the last order and the order of the end point among the plurality of trajectory points when the error is less than a lower error limit value, and updates the start point and the end point by repeating a process of updating the end point as a trajectory point of an order corresponding to a multiplied value obtained by multiplying the updated order of the end point by the predetermined value .
Vehicle control device. the trajectory point setting unit calculates the error by a square error between a distance between a reference point of the vehicle and a center of the arc and a distance between the center of the arc and the target trajectory point;
The vehicle control device according to claim 1. the trajectory model calculation unit calculates the predetermined amount based on a radius of the arc and a wheelbase of the vehicle, and calculates the square error by calculating the square of the difference between the square of the distance between the reference point and the center of the arc and the square of the distance between the center of the arc and the target trajectory point.
The vehicle control device according to claim 2. A vehicle control device
Acquire a predetermined number of trajectory points constituting a target trajectory that the vehicle will pass in the future, in order of proximity from a reference point of the vehicle;
From the plurality of trajectory points, a start point and an end point to which a trajectory model that is a circular arc is to be fitted are set;
Calculate the radius of the arc that fits to the target trajectory point between the start point and the end point among the plurality of trajectory points according to a predefined formula , calculate the center of the arc as a point shifted from the reference point by a predetermined amount based on the radius, and calculate the trajectory model by calculating the error between the distance between the reference point of the vehicle and the center of the arc and the distance between the center of the arc and the target trajectory point;
A vehicle control method for running the vehicle along the calculated trajectory model, comprising :
setting a trajectory point corresponding to the last order among the orders of the plurality of trajectory points as an initial value of the end point, setting a trajectory point in an order corresponding to a multiplied value obtained by multiplying the number of the plurality of trajectory points by a predetermined value greater than zero and less than one as an initial value of the start point, and updating the start point and the end point based on the error;
the reference point is a point on the vehicle where the arc begins;
if the error is greater than an upper error limit value, the end point is updated as a trajectory point corresponding to an order between the first order and the order of the end point among the plurality of trajectory points, and the start point is updated as a trajectory point of an order corresponding to a multiplication value obtained by multiplying the updated order of the end point by the predetermined value , whereas if the error is less than a lower error limit value, the end point is updated as a trajectory point corresponding to an order between the last order and the order of the end point among the plurality of trajectory points, and the start point is updated as a trajectory point of an order corresponding to a multiplication value obtained by multiplying the updated order of the end point by the predetermined value, thereby updating the start point and the end point.
A vehicle control method. A processor of the vehicle control device
acquiring a predetermined number of trajectory points constituting a target trajectory along which the vehicle will pass in the future, the trajectory points being arranged in order of proximity to a reference point of the vehicle;
From the plurality of trajectory points, a start point and an end point to which a trajectory model that is a circular arc is to be fitted are set;
Calculate the radius of the arc that fits to the target trajectory point between the start point and the end point among the plurality of trajectory points according to a predefined formula , calculate the center of the arc as a point shifted from the reference point by a predetermined amount based on the radius, and calculate the trajectory model by calculating an error between the distance between the reference point of the vehicle and the center of the arc and the distance between the center of the arc and the target trajectory point;
A program for running the vehicle along the calculated trajectory model,
a trajectory point corresponding to the last order among the orders of the plurality of trajectory points is set as an initial value of the end point, a trajectory point in an order corresponding to a multiplied value obtained by multiplying the number of the plurality of trajectory points by a predetermined value greater than zero and less than one is set as an initial value of the start point, and the start point and the end point are updated based on the error;
the reference point is a point on the vehicle where the arc begins;
if the error is greater than an upper error limit value, the end point is updated as a trajectory point corresponding to an order between the first order and the order of the end point among the plurality of trajectory points, and the start point is updated as a trajectory point of an order corresponding to a multiplication value obtained by multiplying the updated order of the end point by the predetermined value , whereas if the error is less than a lower error limit value , the end point is updated as a trajectory point corresponding to an order between the last order and the order of the end point among the plurality of trajectory points, and the start point is updated as a trajectory point of an order corresponding to a multiplication value obtained by multiplying the updated order of the end point by the predetermined value, thereby updating the start point and the end point.
program.

Images