1 Introduction

Automated vehicle is recently developed in the field of automotive engineering, robotics, and artificial intelligence for the purpose of commercially available vehicle [1, 2]. The automated vehicle is expected to reduce traffic accidents, traffic jam and driver’s burden. Nowadays, some driving assistant systems such as adaptive cruise control, lane keeping assist and emergency braking system, are available for expressway driving and emergency system.

To achieve urban automated driving, it is important to generate appropriate behaviors according to surrounding circumstances. The automated vehicles are generally equipped with various sensors including light detection and ranging (LiDAR), radar, camera, global navigation satellite system/inertial navigation system (GNSS/INS) as shown in Fig. 1a. These sensors data are processed to percept surrounding environment, estimate a precise location, optimize trajectories and control the vehicle. To implement a robust vehicle system, one common approach is to achieve such perception and decision-making systems using a precise predefined map. These approaches generally refer to highly accurate landmark information (e.q., lane markings, intersections) from the predefined map based on the current vehicle pose as shown in Fig. 1b. This study is focused on a behavior planning for vehicle maneuver in urban traffic scenes.

The behavior planning in automated driving consists of route planning and trajectory planning. The route planning optimizes the minimum cost path from the current position to the destination. Generally, Dijkstra’s and A* algorithms are able to find the shortest route in structured environments from digital map like an automotive navigation system. On the other hand, in unstructured environments, heuristic methods are able to optimize collision-free and smooth path in the free region [3,4,5]. For the obtained path, the trajectory planning is performed to optimize the minimum cost polynomial function [6,7,8,9]. In the trajectory planning, the 4th/5th order polynomial functions enable to generate continuous trajectories with the minimum acceleration/jerk. Primitive driving patterns such as distance keeping and velocity keeping, was described in [6]. The obtained behaviors were then evaluated qualitatively by numerical simulation. Although the primitive driving patterns could behave appropriately while smoothly avoiding obstacles in the surroundings, it was not a natural driving behavior because it was too aggressive. In other words, there is a problem that a large acceleration/deceleration occurs at the time of switching between the distance keeping and the velocity keeping. Driving with large acceleration/deceleration is an unnatural behavior. Therefore, this paper proposes a trajectory generation method that extends the existing method described in [6]. The proposed method can generate smooth trajectory with small acceleration/deceleration by introducing an intermediate behavior. Numerical experiments show that the vehicle generates smooth behaviors according to the surrounding vehicles.

The rest of this paper is composed as follows. Section 2 introduces the behavior model. Section 3 explains the proposed trajectory generation method. Section 4 describes numerical experiments and Sect. 5 concludes with the obtained results.

Fig. 1
figure 1

Overview of experimental vehicle and decision making scheme. a Experimental automated vehicle, b digital map data, c decision making scheme

Full size image

2 Behavior model

2.1 Decision-making system

In our automated vehicle system, the decision-making system consists of multi-level of behavior planners as shown in Fig. 1c. It consists of a route navigation (high level), a driving permission planning according to traffic rules (middle level) and a trajectory planning (low level). The high-level planner has the role of the route planning using the digital map. The middle-level planner manages the traffic rules (e.g., velocity limit, permission of transiting intersection and passing lanes) for the obtained path. The low-level planner then generates a trajectory according to static and dynamic obstacles around the vehicle. Therefore, lateral and longitudinal profiles are optimized based on the obtained path from the high-level planner, and driving permissions from the middle-level planner.

2.2 Sensing data

The above-mentioned planners require sensing information about ego-vehicle and surrounding objects. GNSS/INS and the self-localization module provide precise position, orientation, velocity and acceleration for the ego-vehicle [10,11,12]. Surrounding objects are recognized using the onboard sensors such as LiDAR, radar and camera. For dynamic objects, information about position, orientation, rectangular size, velocity, and acceleration are estimated using LiDAR or radar (the size is only estimated by LiDAR). Static objects are estimated as an occupancy grid map by LiDAR.

3 Trajectory planning

3.1 Frenet-frame coordinate

Traveling path is defined by parametric curves such as Bezier curve and B-Spline curve. To generate trajectories along the shape of the given path, the Frenet-frame is commonly used in [6]. On the Frenet coordinate system, normal trajectory d(t) and tangential trajectory s(t) are defined for the given curve as shown in Fig. 2a. Let d(t) and s(t) be a offset pattern and a distance pattern, respectively. The offset pattern corresponds to the amount of lateral movement within the traveling lane. On the other hand, the distance pattern corresponds to the acceleration/deceleration profile. Therefore, the vehicle trajectory is formulated in the following equation.

$$\begin{aligned} \overrightarrow{x}(s(t), d(t)) = \overrightarrow{r}(s(t)) + d(t)\overrightarrow{n}_r(s(t)). \end{aligned}$$
(1)

In practical use, the optimum trajectory is searched while adjusting the various types of terminal condition for each pattern as shown in Fig. 2b.

Fig. 2
figure 2

Trajectory generation. a Trajectory generation in the Frenet-frame, b offset patterns and distance patterns

Full size image

3.2 Polynomial trajectory

The vehicle trajectory is a combination of an offset pattern d(t) and a distance pattern s(t). Both patterns are generally formulated as a 4th/5th order polynomial function. The 4th order function enables to generate the minimum acceleration trajectory. The 5th order function enables to generate the minimum jerk trajectory. The offset pattern d(t) and the distance pattern s(t) are defined as follows.

$$\begin{aligned} d(t)&= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5, \end{aligned}$$
(2)
$$\begin{aligned} s(t)&= b_0 + b_1 t + b_2 t^2 + b_3 t^3 + b_4 t^4 + b_5 t^5, \end{aligned}$$
(3)

where variable \(a_i\) and \(b_i\) are computed according to the initial conditions \([d_0, \dot{d}_0, \ddot{d}_0]/[s_0, \dot{s}_0, \ddot{s}_0]\) and terminal conditions \([d_1, \dot{d}_1, \ddot{d}_1, \varDelta T_\mathrm{{d}}]/[s_1, \dot{s}_1, \ddot{s}_1, \varDelta T_\mathrm{{s}}]\) at time \(t_1\) (\(t_1=t_0+\varDelta T_\mathrm{{d}}\) or \(\varDelta T_\mathrm{{s}}\)). Let \(\varDelta T_*\) be a preview time (\(\varDelta T_{\mathrm{{min}}}< \varDelta T_* < \varDelta T_{\mathrm{{max}}}, \varDelta T_{\mathrm{{min}}}=2.0[s], \varDelta T_{\mathrm{{max}}}=6.0[s]\)).

The offset pattern d(t) is defined using a quintic function to control the lateral position precisely. The distance pattern s(t) is defined using a quartic or quintic functions for velocity keeping and distance keeping, respectively. To control a velocity smoothly, the distance pattern is generated by solving a quartic function (in this case, \(b_5=0.0\)). On the other hand, to control a distance to the leading vehicle precisely, a quintic function provides the distance pattern.

3.3 Offset pattern generation

To generate collision-free trajectories for static and dynamic objects, trajectories of several terminal conditions are generated as candidates according to the lane with. The trajectory is defined between the initial condition \([d_0, \dot{d}_0, \ddot{d}_0]\) at time \(t_0\) and the terminal condition \([d_1, \dot{d}_1, \ddot{d}_1, \varDelta T_\mathrm{{d}}]\) at time \(t_1\) using a quintic function Eq. (2). Considering a comfortable driving, velocity and acceleration for lateral direction should be minimized. Therefore, the terminal condition is \([d_1, \dot{d}_1, \ddot{d}_1,\) \(\varDelta T_\mathrm{{d}}] = [\varDelta d, 0, 0, \varDelta T_\mathrm{{d}}]\). Several patterns of \(\varDelta d\) and \(\varDelta T_\mathrm{{d}}\) are selected to generate candidate trajectories.

Fig. 3
figure 3

Distance patterns (\(\varDelta s = 0.0\) and \(\varDelta \dot{s} = 0.0\)). a Track mode, b stop mode, c cruise mode, d adjust mode

Full size image

3.4 Distance pattern generation

The distance pattern has the role of velocity profile according to the surrounding objects. Distance keeping and velocity keeping behaviors are generated depending on the distance to the leading vehicle and so on. In [6], track mode, stop mode and cruise mode was proposed as primitive behavior patterns. The track and stop mode are basic distance keeping behaviors which precisely adjust vehicular gap to the leading vehicle and distance to the stop line using quintic functions in Eq. (3). In the distance keeping, it is necessary to consider continuity of position, velocity and acceleration. The initial condition \([s_0, \dot{s}_0, \ddot{s}_0]\) at time \(t_0\) and terminal condition \([s_1+\varDelta s, \dot{s}_1, \ddot{s}_1, \varDelta T_\mathrm{{s}}]\) at time \(t_1\) are then determined to generate trajectories. Let \(\varDelta s\) be a small value of displacement around the terminal condition.

On the other hand, the cruise mode is defined as a velocity keeping behavior. In the velocity keeping, it is not necessary to adjust the terminal position precisely. Because it leads to quick acceleration and deceleration. Therefore, when there is no preceding vehicle nearly, the distance pattern is generated using a quartic function in Eq. (3) under \(b_5=0.0\). The initial condition \([s_0, \dot{s}_0, \ddot{s}_0]\) at time \(t_0\) and terminal condition \([\dot{s}_1+\varDelta \dot{s}, \ddot{s}_1, \varDelta T_\mathrm{{s}}]\) at time \(t_1\) are then determined to generate trajectories.

Werlind et al. [6] demonstrated the collision-free maneuver by numerical simulation. However, the obtained behavior was not a natural driving behavior because a large acceleration and deceleration occurs at the time of switching from the velocity keeping to the distance keeping. Therefore, this paper proposes a trajectory generation method that extends the existing method in [6] to generate more natural behavior with small acceleration and deceleration. This paper introduces an intermediate behavior “adjust mode” to gradually switch from the velocity keeping to the distance keeping.

Figure 3 illustrates an overview of distance patterns. Figure 4 indicates a flowchart of the mode selection. Leading vehicle is extracted for each offset pattern. Suitable mode candidates are extracted based on the position, velocity and acceleration for the ego-vehicle and the leading vehicle. Trajectories is generated according to each mode. If multiple mode is selected, the model with the closest terminal position is extracted. In Fig. 4, \(\dot{s}_{tgt}\) is the target velocity. It is the speed limit value of the current road obtained from the digital map. Details of each distance pattern are described as follow.

Fig. 4
figure 4

Distance pattern mode selection

Full size image

3.4.1 Distance keeping: track mode

The track mode generates distance keeping profile for the preceding vehicle as shown in Fig. 3a. The following equation keeps a vehicular gap \(D_{\mathrm{{des}}}(t_1)\) between the ego-vehicle and the preceding vehicle while aligning the velocity and the acceleration.

$$\left\{ \begin{gathered} s_{1} = s_{{{\text{lv}}}} (t_{1} ) - D_{{{\text{des}}}} (t_{1} ) \hfill \\ \dot{s}_{1} = \dot{s}_{{{\text{lv}}}} (t_{1} ) + \tau \ddot{s}_{{{\text{lv}}}} (t_{1} ) \hfill \\ \ddot{s}_{1} = \ddot{s}_{{{\text{lv}}}} (t_{1} ) \hfill \\ D_{{{\text{des}}}} (t_{1} ) = D_{0} + \tau \dot{s}_{{{\text{lv}}}} (t_{1} ) \hfill \\ s_{{{\text{lv}}}} (t_{1} ) = s_{{{\text{lv}}}} (t_{0} ) + \dot{s}_{{{\text{lv}}}} (t_{0} )\Delta T_{{\text{s}}} + \ddot{s}_{{{\text{lv}}}} (t_{1} )\Delta T_{{\text{s}}}^{2} /2 \hfill \\ \dot{s}_{{{\text{lv}}}} (t_{1} ) = \dot{s}_{{{\text{lv}}}} (t_{0} ) + \ddot{s}_{{{\text{lv}}}} (t_{0} )\Delta T_{{\text{s}}} \hfill \\ \ddot{s}_{{{\text{lv}}}} (t_{1} ) = \ddot{s}_{{{\text{lv}}}} (t_{0} ) \hfill \\ t_{1} = t_{0} + \Delta T_{{\text{s}}} \hfill \\ \end{gathered} \right.,$$
(4)

where \(s_{\mathrm{{lv}}}\) is a position of the preceding vehicle. \(D_0\) is the minimum vehicular gap (\(D_0 = 5\) m) and \(\tau\) is an inter-vehicle time (\(\tau =2\) s).

3.4.2 Distance keeping: stop mode

To park the vehicle at the stop line or the destination, the stop model generates a deceleration profile as shown in Fig. 3b. \(s_{\mathrm{{stop}}}\) is a distance to the parking place which is given from the middle-level planner.

$$\left\{ \begin{gathered} s_{1} = s_{{{\text{stop}}}} \hfill \\ \dot{s}_{1} = \ddot{s}_{1} = 0.0. \hfill \\ \end{gathered} \right.$$
(5)

3.4.3 Velocity keeping: cruise mode

The cruise mode keeps the ego-velocity to the given velocity \(\dot{s}_{\mathrm{{tgt}}}\) as shown in Fig. 3c. \(\dot{s}_{\mathrm{{tgt}}}\) is determined from the speed limit.

$$\left\{ \begin{gathered} \dot{s}_{1} = \dot{s}_{{{\text{tgt}}}} \hfill \\ \ddot{s}_{1} = (\dot{s}_{1} - \dot{s}_{0} )/\Delta T_{{\text{s}}} \hfill \\ \end{gathered} \right.$$
(6)

3.4.4 Velocity keeping: adjust mode

When the preceding vehicle is at a long distance, the adjust mode gradually reduces the vehicular gap to \(D_{\mathrm{{des}}}(t_1)\) as shown in Fig. 3d. It searches the maximum velocity \(\dot{s}_{\mathrm{{adj}}}\) with the maximum preview time \(\varDelta T_\mathrm{{s}}\) under \(D_{\mathrm{{des}}}(t_1) < D(t_1)\).

$$\left\{ \begin{gathered} \dot{s}_{1} = \dot{s}_{{{\text{adj}}}} \hfill \\ \ddot{s}_{1} = 0.0 \hfill \\ T_{{{\text{min}}}} = t_{0} + \Delta T_{{{\text{min}}}} \hfill \\ T_{{{\text{max}}}} = t_{0} + \Delta T_{{{\text{max}}}} . \hfill \\ \end{gathered} \right.$$
(7)
Fig. 5
figure 5

Collision detection. a Collision detection for dynamic object, b collision detection for static object using occupancy grid map

Full size image

3.5 Collision detection

For all generated trajectory set, it is determined whether or not each trajectory is drivable. For dynamic objects, it is confirmed the overlapping of bounding boxes at each time step as shown in Fig. 5a. When the rectangular boxes of the ego-vehicle and the moving object overlap, it is judged that a collision has occurred. In a similar way, a collision detection with static object is performed using the occupancy grid map. Each bounding box of the ego-trajectory is confirmed a collision with the obstacle grid cell of the occupancy grid map as shown in Fig. 5b.

3.6 Optimal trajectory selection

Collision-free trajectory set are generated based on the above-mentioned procedures. The minimum cost trajectory is selected from these trajectories [13]. The cost function C is defined in Eqs. (8)–(10).

$$\begin{aligned} C&= k_{\mathrm{{path}}}C_{\mathrm{{path}}}^{i} + k_\mathrm{{d}} C_\mathrm{{d}} + k_\mathrm{{s}} C_\mathrm{{s}}, \end{aligned}$$
(8)
$$\begin{aligned} C_\mathrm{{d}}&= k_j^\mathrm{{d}} \int ^{t_1}_{t_0} \dddot{d}^2 (t) \mathrm{{d}}t + k_x^\mathrm{{d}} \varDelta T_\mathrm{{d}} + k_y^\mathrm{{d}} \varDelta d^2, \end{aligned}$$
(9)
$$\begin{aligned} C_\mathrm{{s}}&= \left\{ \begin{array}{l} k_j^\mathrm{{s}} \int ^{t_1}_{t_0} \dddot{s}^2 (t) \mathrm{{d}}t + k_x^\mathrm{{s}} \varDelta T_\mathrm{{s}} + k_y^\mathrm{{s}} \varDelta s^2 \quad \text{(if } \text{ distance } \text{ keeping) }\\ k_j^\mathrm{{s}} \int ^{t_1}_{t_0} \dddot{s}^2 (t) \mathrm{{d}}t + k_x^\mathrm{{s}} \varDelta T_\mathrm{{s}} + k_y^\mathrm{{s}} \varDelta \dot{s}^2 \quad \text{(if } \text{ velocity } \text{ keeping) },\\ \end{array} \right. \end{aligned}$$
(10)

where, \(C_\mathrm{{d}}\) and \(C_\mathrm{{s}}\) are costs about the offset and the distance patterns, respectively. Both terms evaluate integral of jerks, preview times and displacements of terminal condition. \(C_{\mathrm{{path}}}^{i}\) is a weight of drivable lanes for the i-th lane. \(C_{\mathrm{{path}}}^{i}\) is determined from the middle-level planner to enable to change lanes for a multiple-lane road. In case of \(C_{\mathrm{{path}}}^0 < C_{\mathrm{{path}}}^1\), the trajectory traveling in 0-th lane tends to be selected as shown in Fig. 6. On the other hand, the trajectory to change the lane is likely to be selected in case of \(C_{\mathrm{{path}}}^0> C_{\mathrm{{path}}}^1\).

Fig. 6
figure 6

Multiple lane driving

Full size image

4 Simulation and evaluation

4.1 Conditions

Two types of experiments are carried out to evaluate the effectiveness of the proposed method. The first experiment quantitatively evaluates the maximum value of acceleration/deceleration depending on whether or not the proposed intermediate mode (adjust mode) is introduced. As one of the scenes where large deceleration occurs, the scene approaching the stopped vehicle while traveling at the maximum speed of the general road (60 km/h) is evaluated as shown in Fig. 7a. The second evaluation qualitatively observes the behavior for typical driving scenes. The obtained behaviors are observed with typical driving maneuvers of following and overtaking scenarios as shown in Fig. 8a, b.

Fig. 7
figure 7

Experimental results. a Scenario, b results: w/o adjust mode, c results: w/ adjust mode

Full size image
Fig. 8
figure 8

Experimental results. a Following scenario, b overtaking scenario, c results: following scenario, d results: overtaking scenario

Full size image

4.2 Results

Figure 7b, c indicate experimental results for the approaching driving. In the case where the adjust mode is not used, it can be confirmed that a large deceleration occurs immediately after switching from the velocity keeping to the distance keeping. In Fig. 7b, the maximum deceleration value was \(-3.94\;\text{m}/\text{s}^2\). On the other hand, by introducing the adjust mode, it was confirmed that deceleration started from a few seconds before and the maximum deceleration value was suppressed to \(-1.71\;\text{m}/\text{s}^2\) as shown in Fig. 7c. Therefore, the results showed that the introduction of the proposed method is possible to reduce the maximum deceleration value to 43.4% of the conventional one.

Figure 8c, d shows experimental results for typical driving maneuvers. In the following scenario, the preceding vehicle starts to decelerate after 10 s and stops as shown in Fig. 8c. While the gap is increasing, the ego-vehicle drives the cruise mode to keep the target velocity. When the preceding vehicle starts decelerating, the ego-vehicle gradually reduces the gap using the adjust mode. The track mode then accurately controls the gap. In a series of state transitions, it is observed that the ego-velocity and the gap are smoothly updating. On the other hand, the preceding vehicle parks by the side of the road in the overtaking scenario. As shown in Fig. 8d, the ego-vehicle smoothly controls the lateral offset and velocity during overtaking. These results show that the proposed planner controls the vehicle properly according to surroundings.

5 Conclusion

A method of trajectory optimization is proposed for vehicle maneuvers in urban road. Polynomial curves generate the minimum jerk curve in given conditions. According to the static and dynamic objects, the ego-vehicle switches motion pattern smoothly. The proposed method introduces the intermediate behavior to smoothly switch from the velocity keeping behavior to the distance keeping behavior. Experimental results showed that by introducing the proposed method, it is possible to halve the deceleration value that occurs when the ego-vehicle is approaching a stationary vehicle.

Additionally, the proposed trajectory planner has been installed in our automated vehicle and conducted a self-driving test in real traffic scenes. Further quantitative evaluations in dense traffic scenes in actual automated driving are future works.