A Hierarchical Motion Planning Framework for Autonomous Driving in Structured Highway Environments

This paper presents an efficient hierarchical motion planning framework with a long planning horizon for autonomous driving in structured environments. A 3D motion planning with time information is a non-convex problem because there exists more than one local minimum point and various constraints such as roads and obstacles. Accordingly, to deal with high computational complexity and the problem of falling into a local minimum in an enormous solution space, a decoupled method is utilized, that consists of two steps: Long-term planning and short-term planning. First, the long-term planner provides reasonable far-sighted behavior through two processes. In the first process, a rough path that includes a driving strategy is generated in the 2D spatial space. Then, the jump point search algorithm is utilized with time information on the path to reduce the computational burden of A*, giving an acceptable quality of solution at the same time. In this step, a safe, comfortable, and dynamically feasible trajectory is generated. Next, the short-term planner optimizes a short-sighted trajectory using particle swarm optimization. In this method, a steering angle set is encoded as a particle, resulting in a safe, comfortable, and kinodynamically feasible trajectory. The proposed algorithm is implemented and evaluated in a range of vehicle-in-the-loop simulation scenarios, which include various virtual static and dynamic obstacles generated by Intelligent Driver Model. In the evaluation results, the proposed method reduced the computation time by up to 0.696 s with increasing the step cost by up to about 3%. The proposed algorithm is executed every 100 ms for a planning horizon of 10 seconds, and the average computation time is 31 ms with the worst-case computation time of 94 ms.


I. INTRODUCTION
During the last few decades, research on autonomous driving has been actively studied worldwide, both in academia and the industry [1], [2] to aid drivers and, reduce tedious drivingrelated tasks. It also aims to prevent accidents caused by carelessness of drivers, which accounts for the majority of casualties [3], [4]. As a core module and being the decision stage in autonomous driving, the motion-planning module generates a legal, safe, comfortable, and kinodynamically feasible trajectory considering the surrounding environments and vehicle states. To date, many motion planning algorithms have been developed.

A. RELATED WORK
The motion planning of an autonomous vehicle can be divided into two approaches [5]: Direct planning methods and decoupled planning methods based on whether the state space of the configuration space is decoupled.
In direct methods, the optimal solution is selected instantaneous through optimal planning using searching or optimization approaches in a spatiotemporal state space, which includes time information along with longitudinal and lateral motions. Ziegler et al. [6] used a method that transforms a nonconvex problem into a convex problem considering the comfort, safety, and smoothness of the trajectory. Then, using a sequential quadratic programming (SQP) algorithm, VOLUME 4, 2016 a nonlinear optimization problem is solved numerically. It is well-known that the SQP algorithm should have a good initial value for an acceptable computation time [7]. Chen et al. proposed the constrained iterative linear-quadratic regulator (CILQR) to efficiently solve the optimal control problem with nonlinear system dynamics and a general form of constraints [8]. The computational efficiency of CILQR was shown to be much higher than that of the SQP solver. However, without a good initial value, it can also influence the convergence speed of the algorithm.
For other approaches of direct methods, spatiotemporal state lattice-based trajectory planning methods were introduced to generate a feasible trajectory in a dynamic scenario [9], [10]. The choice of resolution of the search space is a trade-off between computation time and solution quality. If the resolution increases, the computation time increases. If the resolution decreases, the optimality of the solution space may be degraded, and a feasible solution cannot be found. Dorff et al. proposed a trajectory planning and control method using nonlinear model predictive controller (NMPC) [11]. It is validated in partially occluded parking environments with safe planning horizon of 1 s to 1.5 s. In addition, in [12] and [13], MPC-based trajectory planning methods were used. MPC solves a sequence of finite-time optimization problems in a recursive manner and generates consecutive control actions regarding the vehicle's motion. A comfortable trajectory is generated while guaranteeing the safety of an autonomous vehicle. However, this method shows poor performance for non-convex and high-complexity problems.
Conversely, decoupled methods attempt to generate an optimal solution through multiple steps by reducing the dimensions of the state space. Werling et al. [14] generated longitudinal and lateral trajectories separately considering dynamic obstacles in the Frenet coordinate and then selected one optimal trajectory. Among the generated trajectories using quartic and quintic polynomials versus time, the jerk-minimizing trajectory is chosen. This method has the drawback of frequent swerving motion due to shortsighted planning. In [15] and [16], Li et al. performed a path-speed decomposition method to obtain an optimal trajectory step by step. First, a spatial path was generated using a curvature polynomial, and then a trapezoidal velocity profile was smoothed using a polynomial spline to obtain a trajectory. This method has the drawback of a short planning horizon and incompleteness of the solution owing to the finite set of motion primitives. In [17], a combination of dynamic programming and quadratic programming was proposed to generate path and speed profiles, respectively. A 3D stationlateral-speed problem is transformed into two 2D stationlateral and station-speed problems to reduce the complexity of the problem and computation time. Unfortunately, a limitation is that the generated trajectory is not guaranteed to be kinematically feasible. Xu et al. [18] used a method that consisted of two parts: Trajectory planning and trajectory optimization. First, a rough path and speed profile are generated, and then, they are iteratively optimized. However, non-holonomic constraints are not guaranteed to be satisfied. Zhang et al. proposed a method which adopts several steps [19]. In the first step, in a Frenet frame, a smooth driving guide line is obtained, then, optimization is performed for path generation. The second strategy is the proposition of piecewise-jerk path formulation. Finally, the optimization is performed to search a safe and kinematically feasible solution considering obstacles.
In addition, Li et al. proposed a semantic-level maneuver decision-making and trajectory planning method [20]. After the upper-level maneuver is decided, the lower-level trajectory planning is decoupled into longitudinal and lateral directions. Then, the selected trajectory is optimized by the operator splitting quadratic program (OSQP). This method is validated using Prescan simulator under two scenarios. Zhang et al. [21] introduced a hybrid trajectory planning method that generates a smooth, safe, and computationally efficient solution. However, only static obstacles were considered in their work. Lim et al. [22] used a hierarchical strategy in which a sampling-based approach was used for behavioral planning, and optimization was conducted for trajectory planning. However, the planner lacks the capability to generate trajectories with long horizons. Jin et al. [23] proposed a method to adaptively change the longitudinal horizons considering obstacles for better performance of onroad autonomous driving with avoidance of both static and moving obstacles. In decoupled methods, static obstacle is typically considered in the path planning stage and dynamic obstacle is considered in the speed planning stage. Thus, it is not guaranteed that the decoupled method is optimal. In addition, the number of tuning parameters is usually much more than the direct methods and their adjustment becomes difficult.

B. CONTRIBUTION
Nonetheless, a decoupled method was used for motion planning in our work to reduce the problem complexity and computational burden. Motion planning for self-driving can be divided into two parts [24], behavioral planning and trajectory planning. Behavioral planning can be categorized as high-level decision-making and is responsible for longterm planning (t horizon > 5s) in complex situations [25]. Many studies have been conducted on state machines when performing long-term planning [26]- [30]. Various behaviors are represented as predefined states, and then an appropriate behavior is selected at every step. However, when the situation becomes complicated and the maneuvers to model are increased, it becomes computationally intractable owing to the exponentially growing number of state transition rules. Therefore, to address this problem, Hubmann et al. [25] proposed a generic long-term planning method using the A* graph search algorithm. However, only longitudinal planning was regarded in their work.
In this paper, a hierarchical framework for motion planning in structured environments is proposed that provides a long-term solution that considers both the longitudinal and lateral directions. The algorithm structure is divided into two steps, long-term planning and short-term planning. Longterm planning is performed first and consists of two processes. In the first process, an optimal spatial path is obtained by considering the road geometry and obstacles. This part determines which space to drive is desirable in a complex situation. In the next process, a spatiotemporal trajectory is generated by placing the position and velocity profiles versus time on the previously obtained long-term path, which has a reduced searching space. This process provides a legal, safe, comfortable, and dynamically feasible long-term maneuver. In the second step, short-term planning is performed using the front part of the long-term solution. This step provides a safe, comfortable, and kinodynamically feasible trajectory in a combined resampling and optimization manner. Finally, the performance of the proposed algorithm is verified through a vehicle-in-the-loop simulation (VILS) with a real car under various scenarios. The contributions of this study can be summarized as follows: • The proposed hierarchical scheme can generate safe, comfortable, and kinodynamically feasible trajectories that can deal with static and dynamic obstacles. • Jump point search (JPS) algorithm which has been used only for the problems with a uniform step cost, is utilized in our problem which has a non-uniform step cost with the help of a carefully designed assumption. Leveraging JPS algorithm reduces the computation time significantly and allows real-time long-term planning with an acceptable solution quality. • The short-term trajectory planner optimizes the initial trajectory from the long-term planner. Furthermore, steering angle sets are encoded as particles in particle swarm optimization (PSO) through a kinematic vehicle model that satisfies the non-holonomic constraint. Additionally, in PSO, the driving comfort, safety, and dynamic constraints are considered as well. • The feasibility of the proposed method was demonstrated in various automated driving scenarios using VILS.

C. PAPER ORGANIZATION
The remainder of this paper is organized as follows: The overall algorithm framework is introduced in Section II. The optimal long-term path planning using the shortestpath algorithm and long-term trajectory planning using a JPS algorithm are described in Section III. In Section IV, short-term trajectory optimization using PSO algorithm is explained. Section V explains the experimental setup and evaluation of the proposed algorithm in a vehicle-in-the-loop environment. Section VI summarizes the proposed algorithm and discusses the future work.

II. ALGORITHM FRAMEWORK
To generate a trajectory in an autonomous driving situation that is non-convex [31], the autonomous vehicle must consider complex driving environments which comprise static and dynamic surrounding vehicles, roads, traffic rules, etc. Based on the scene information, an autonomous vehicle must run within the normal speed range, change lanes, and drive safely without colliding with surrounding vehicles. Therefore, when a trajectory is generated, safety, comfort, and limits of the vehicle kinematics and dynamics should be considered. The overall algorithm framework for optimal motion planning is shown in Fig. 1. This study focuses on a hierarchical motion planning framework for autonomous driving on unidirectional roads. The information of the surrounding vehicle states from the perception module is assumed to be available to the planning module.
In the first step, the long-term (∼10s) spatial path including the longitudinal and lateral motions, is obtained by considering the surrounding vehicles. A maneuver with a lane-level lateral motion target is generated by considering the current state and prediction during a specific time horizon of the surrounding environments.
In the second step, the position and velocity profiles are applied to the optimal spatial path from the first step to obtain a long-term spatiotemporal trajectory. Safe motion planning is carried out in which a collision-free trajectory is generated considering static and dynamic obstacles. The improved A* algorithm, JPS, was utilized as the base algorithm to reduce the computational effort [32]. The original JPS is optimized for solving the grid-based problem, which has a uniform cost for each action. In our problem, which is without a uniform cost for each action, a sub-optimality condition is defined and the JPS is applied.
Finally, short-term trajectory optimization was performed using the front-part of the optimal long-term trajectory. It is of practical importance to follow the adjacent short-term (∼3s) trajectory when tracking the control of the generated trajectory. In our work, the PSO algorithm was utilized to make the short-term trajectory kinodynamically feasible, smooth, and safe.

III. LONG-TERM PLANNING
In this section, the concept of long-term planning of the proposed hierarchical framework is explained in detail. First, the optimal spatial path generation is described, then the VOLUME 4, 2016 spatiotemporal trajectory using the optimal spatial path is described.

A. OPTIMAL SPATIAL PATH GENERATION
To create an optimal spatial path, a search space is created that comprise lane-level nodes. An optimal path selection process was performed on the search space. Each node is represented by the following 3D space as state n.
where s and d are defined as the longitudinal and lateral positions along the road, respectively, l is defined as the rightmost lane having a value of 1 and the leftmost lane having a value of N l . The road geometry was defined according to the Frenet coordinates, as described in Fig. 2. The search space consists of a total of N p × N l nodes in the Frenet coordinates. In addition, except for the initial node n 0 , the k th step longitudinal position s k of node n k is expressed as follows: where v o and s 0 are the initial velocity and longitudinal position of the ego vehicle, respectively, and ∆t lt is the time interval. ∆s is determined by the current speed of the ego vehicle. However, to prevent the search space from disappearing under low speeds, ∆s is set to be greater than ∆s min , which is set to 10 m in our work. Fig. 3 shows an example of the optimal spatial path generation. The blue vehicle is the ego autonomous vehicle and the white vehicle represents the obstacle vehicle during the prediction horizon. The nodes were placed in the direction of the road with an interval of ∆s. Each node is generated at every layer, and the result is defined as a directed, acyclic graph. The edge is a linear path from a node in one layer to a node in the next layer. The cost of edge e n k →n k+1 is shown in the following equation, where n k is a node in layer L k and n k+1 is a node in layer L k+1 . J(e n k →n k+1 ) = w dist dist(e n k →n k+1 ) + w c c(e n k →n k+1 ) + w col col(e n k →n k+1 ) (4) where w dist , w c , w col are weights for each index.
The first cost term represents the length of each edge using the Euclidean distance between two nodes, and it prevents the ego vehicle from changing lanes frequently by assigning a penalty to a long edge, as shown in (5).
The second cost term is for the consistency of consecutive plans. This term reduces the difference in the lateral offset between the current path candidate and the previous optimal spatial path, as described in (6): where d prev k+1 and d k+1 are the lateral positions of the previously selected optimal path and a new path candidate, respectively.
The velocity profile of the previous planning cycle was used to calculate the predicted position of the ego vehicle. It is assumed that using the velocity profile of the previous planning cycle is reasonable because the planning frequency is sufficiently high. The last collision cost term suppresses the collision between the predicted ego vehicle and the surrounding vehicles predicted with the constant velocity (CV) model and is defined as Here, subscript j denotes a number from 1 to N o where N o is the number of obstacles in the region of interest. For each j from i = k s to i = k c , it is checked if the condition in (7) is satisfied. If the condition is satisfied even once, cost c col is imposed. Furthermore, k s and k e represent the start and end indices of the s-axis position, where the predicted ego vehicle is within the edge e n k →n k+1 , respectively. Additionally, s e,i and s O j i are the s-axis positions of the predicted ego vehicle and j th obstacle O j i at the i th step, respectively. Further, l i and l O j i are the lane values of nodes n i and j th obstacle, O j i , at the i th step, respectively. A constant value c col is set sufficiently large so as to make it dominate the overall cost function for penalizing collisions. The lengths of the obstacle and ego vehicle are L O and L ego , respectively. The margin distance value is s margin , and s sf is the safety distance to restrict collisions between vehicles.
This becomes a shortest-path problem, in which dynamic programming is utilized to solve it quickly. The shortestpath problem is an algorithm for finding the shortest path between nodes in a graph. In our problem, one node per layer is selected to construct the path using the cost function in (4). The optimal solution with minimum total cost is given by a sequence of nodes on the N p layers covering the space that satisfies:

B. SPATIOTEMPORAL TRAJECTORY GENERATION PROBLEM FORMULATION
A spatial path planner provides the desired maneuver to a spatiotemporal trajectory planner considering the longterm objective of autonomous driving in the spatial search space. An optimization problem is formulated to generate a trajectory based on the optimal spatial path given, and is represented as: The detailed description is contained in Sections III-B1 to III-B3. State x in the state space X ⊆ R 3 is represented as follows: where the state x includes the longitudinal position s, velocity v and time t. The problem is formulated as a discrete planning problem [33] with a similar approach as in [25] and is solved using the JPS algorithm, which will be explained in detail in Section III-C.

1) Transition Model
The state x k represents a state in the planning step k, and the state transition model in discretized form is expressed as follows: where a k is an acceleration candidate belonging to a discretized acceleration set A. The state is expanded using the state transition model for a limited time T lt with an interval of ∆t lt .

2) Representation of the Environment
The autonomous vehicle encounters a static and dynamic obstacle O on the road, the information of a specific obstacle at the k th step is parameterized as a vector, Next, a procedure is performed to extract information about the obstacles that affect the ego vehicle. The long-term optimal spatial path obtained is checked to determine the extent of overlap with the prediction of surrounding vehicles. The overlapped obstacle O ol is represented as follows using O which includes the accumulated information of O k during the prediction horizon: where s O ol,s,i and s O ol,e,i represent the start and end positions in the longitudinal direction of the optimal spatial path at the i th overlap with the specific obstacle, respectively. Furthermore, t O ol,s,i and t O ol,e,i represent the time information at the corresponding step, respectively. Fig. 4 shows an example in which one obstacle is overlapped twice on the optimal spatial path. In Fig. 4(a), the predicted ego vehicle (light blue color) through the optimal spatial path (blue solid line) represents the position information for the N p step calculated from the velocity profile of the previous planning cycle.
In Fig. 4(b), the overlapped information is expressed ol,e,i T |i = 1, 2 using (13) where m = 2. It is considered overlapped if the prediction of the surrounding vehicle is in the same lane as the optimal spatial path, and the difference in the longitudinal position between the obstacle and the ego vehicle is within s sf . Assuming highway driving, the variation in s sf owing to the heading angle is ignored. The overlapped path is represented by the red line in Fig. 4(b). At k = 1 and k = 2, the ego vehicle and obstacle overlap. Therefore, it is assumed that the position of s between the two points continues to overlap. Next, at k = 3, the optimal spatial path moves to the left lane and does not overlap with the obstacle at the same time step. Therefore, the first overlap section is set as: [s O ol,s,1 , s O ol,e,1 ]. Next, at k = N p − 1, the optimal spatial path and obstacle do not overlap, whereas at k = N p , they overlap. In this case, utilizing the s positions of the ego vehicle, s position of the obstacle and s sf , the second overlap section is set as represent time information corresponding to each s overlap value.

3) Cost Function
If an action a k is taken to state x k , it moves to the next state x k+1 through the transition model. The step cost c (x k , a k , x k+1 , O ol ) is defined for each path, and the overall cost c tot which is summed along the path is represented as follows [33]: The path with a minimal cost from the initial state to the goal state is obtained using the graph search algorithm. The step cost is defined as where w v and w A are weights for each index. The first cost term represents the deviation from the reference velocity v ref and is defined as follows: The limit velocity v limit = d max κ is determined by the maximum lateral accelerationd max and road curvature κ. Furthermore, there exists a road velocity limit v road . v ref is decided to be the minimum value of these two velocities: The second cost term penalizes the size of the action that belongs to acceleration set A. It increases the driver's comfort by providing cost in the form of squared acceleration.
The last cost term represents the cost of obstacles. An obstacle has an area on the distance-time plane in state space X . The cost for the static and dynamic obstacles is expressed as follows using the overlapped obstacle O ol :  if O is empty then 7: x new = transition(N.x, −a max ) 8: Add N new to O if N new ∈ C then 21: continue 22: else if N new has the same state with 23: n ∈ O with smaller cost then The optimality of JPS is guaranteed only when the step cost is uniform, which is not the case in our study. Nonetheless, to take advantage of JPS in terms of computational efficiency and to carry out long-term planning in real time, the condition for the sub-optimality of the solution is defined as follows: • In the time domain where obstacles overlap within the planning horizon of the ego vehicle, if a = 0 is taken once as the minimum action, only that action can be taken for the rest of the time.
A suboptimal solution is equivalent to the optimal solution if there are no other vehicles on the path, and it further,

Algorithm 2 Function jump
Require: x k : initial state, a k : action, g : goal state, O : Obstacle, C cum : cumulative cost 1: x k+1 ← transition (x k , a k ) 2: if x k+1 is inside an obstacle then collision 3: return x k+1 , c (x k , a k , x k+1 , O) + C cum 4: else if x k+1 = g then goal state 5: if jump (x k+1 , a k , g) has finite cost then 12: shows different movements if there are obstacles within the planning horizon of the ego vehicle. Let us assume that both the ego vehicle and the preceding vehicle on the path have a lower velocity than the reference velocity. In this case, as shown in Fig. 5(a), the optimal solution when w A is low as in our case is as follows: first to accelerate to reach the target velocity of 20 m/s in the example, and then to decelerate near the obstacle. In contrast, as shown in Fig. 5(b), the suboptimal solution is to maintain a = 0 after acceleration. If it collides with the obstacle, as shown in Fig. 5(b), the node lastly accelerated is not added to the open list. Thereafter, the trajectory that maintains a constant speed is chosen as the suboptimal solution.
To an original optimal problem, JPS cannot be utilized because the minimum action that has a minimum step cost is not consistent and changes according to the state and action. However, to solve a suboptimal problem, as in our case, JPS can be utilized because maintaining a constant speed at last is a solution, and the constant speed (a = 0) can be interpreted as the minimum action. In Line 18 of Algorithm 1, the JPS algorithm is performed as described in Algorithm 2, which represents a recursive function that makes a jump between the nodes. The jump step continues until the stop conditions are met [25], which consist of a collision, goal state, and forced neighbor as described in Lines 2 to 7 of the algorithm. In Line 1, the transition model described in (12) is utilized to obtain the next state, x k+1 , using the current state and action. Line 2 indicates the collision situation, Line 4 is when the goal state is reached and Line 6 is when the forced neighbor is encountered. The forced neighbor is represented as x k+1 in Fig. 6, which is the state blocked by obstacles and must be passed through x k [32]. Lines 8 to 11 proceed when the first step's action is not the minimum action. In this case, the jump function recurses with the minimum action and checks if it collides with obstacles. If no collision occurs, the cost has a finite value, and the next state x k+1 is returned. Otherwise, the jump is performed again with the same step action, a k . The search heuristic used in this study is represented as follows: In (22), the action that minimizes the difference between the reference velocity and the state's velocity at k + 1 is calculated. Then, the selected action is applied to obtain v k+1 , which is utilized in (21). This process is repeated from k = t until k = T lt − 1. The heuristic is admissible in (21) and has exactly the same form as the cost c v in the step cost in (16), and it is guaranteed to have a lower value than the minimal cost from k = t to k = T lt − 1 which is expressed In addition, as long as the heuristic is the lower bound on the actual cost and is consistent, the A* algorithm is complete. Consistency is fulfilled if Thus, an admissible and consistent heuristic was used in this study.
Next, to verify the performance of the solution using the JPS, three metrics which comprise the number of nodes in the open list, computation time, and step cost are evaluated under the test scenarios generated in Section V. In Table 1, the metrics are compared with the JPS for the test scenarios. The average value was used to evaluate the first two metrics, and the ratio to the JPS was utilized for the last metric. For the number of nodes in the open list, it is significantly reduced for all scenarios. Accordingly, the computation time was also greatly reduced for each test scenario. Finally, in the worstcase scenario related to the step cost, the optimal solution from A* reaches 0.97 times that of the solution from the JPS, which is acceptable.

IV. SHORT-TERM PLANNING
In this section, the short-term planning of the proposed hierarchical framework is explained. First, to increase the resolution, resampling of the spatiotemporal trajectory obtained from long-term planning is performed. Then, the part that optimizes the trajectory by encoding a steering angle set as a particle is explained using the resampled trajectory as the reference position value in the PSO.

A. RESAMPLING OVER TRAJECTORY
The spatiotemporal trajectory obtained from the long-term planning is a behavioral strategy with a long time horizon. However, tracking a short-term trajectory is important for the control module, therefore, the front part of the entire longterm trajectory was used for the resampling step. The resul-FIGURE 7: Example for a particle swarm encoding with kinodynamic constraints. Among the valid trajectories (black) except for invalid (yellow) trajectories, the optimal trajectory (red) is selected. tant trajectory has sufficient resolution for considering the continuous changes of the vehicle state. Linear interpolation between discrete nodes is used for the resampling step. The planning horizon of T st is discretized by ∆t st resulting in the N pso number of sample nodes.

B. TRAJECTORY OPTIMIZATION
PSO is an algorithm that was inspired by the flocking behavior of swarms [34]. It is well-known that PSO performs well when solving nonlinear optimization problems and has the advantages of a fast convergence speed with only a small number of parameters that need to be adjusted [35]. In addition, it is a very efficient global search algorithm and derivative free. Furthermore, although there is no guarantee that PSO provides the global optimal solution, it is less likely to converge to a local minimum and finds good quality of solution with reasonable computational effort. For this reason, many studies have used the PSO for path planning [35]- [38].
Each particle p i t at time t in the swarm indicates a possible solution in the multidimensional search space. A particle consists of a total of N pso steering angle values and becomes an N pso -dimensional vector. The fitness of the particle is calculated through a designed fitness function, which is described in detail in Section IV-C. Each particle moves with a speed vector v i t , and the trajectory is adjusted accordingly. In addition, the personal best solution p i pbest and swarm's best solution p gbest are updated respectively. Each particle is updated as follows: Then, the velocity of each particle is updated considering the current velocity, the personal best solution x pbest , and the global (the swarm's) best solution x gbest , as follows: where w i is the inertia weight damping ratio, w p and w g are the personal learning coefficient and global learning co- efficient, respectively. Furthermore, r 1 and r 2 are uniformly distributed random numbers.
The optimization phase is terminated when the given optimization criteria are met, or if the maximum iteration limit is reached. Fig. 7 shows an example of a steering angle set encoding as a particle of the PSO algorithm, and the i th particle p i t , is expressed as shown in (25).
A particle consists of a steering angle set of the ego vehicle during horizon N pso . A particle is passed through the kinematic [39], [40] that satisfies the non-holonomic constraint to obtain position information s k , d k , and heading angle θ k as follows: where L is the length of the ego vehicle, and the subscript k denotes a number from 1 to N pso . The obtained position set ensures that the curvature of the trajectory is within the kinematic constraints.

C. FITNESS FUNCTION
In the PSO, particles move in the direction of minimizing the fitness function to find the optimal value. In this study, the fitness function is composed of six terms and is expressed by the following equation.
f pso = w ref f ref +w as f as +w ad f ad +w js f js +w jd f jd +f col (29) where w ref ,w as ,w ad ,w js and w jd are the weights for each index except for the last collision cost.
The first fitness term represents a reference cost and is defined as VOLUME 4, 2016 where s i,ref and d i,ref are the reference nodes for the longitudinal and lateral directions from the resampled behavioral trajectory. This fitness term makes a particle vector from the PSO that does not deviate significantly from the generated trajectory through the node and edge of the road geometry.
The next four fitness terms indicate the acceleration and jerk costs and are related to driving comfort, which are defined as follows: The acceleration and jerk values were calculated using forward finite differences. The physical limits of the vehicle dynamics for safe driving are considered using a circle of forces [41]. The acceleration values used when calculating the acceleration costs f as and f ad were constrained to be within the maximum acceleration:  (35) is violated, the i th cost of f a is set to be infinite to prevent the particle from being selected. The restriction check is not performed in Cartesian coordinates but rather in the Frenet space.
The last fitness term is related to collisions. The vehicle shape is represented using two circles for collision checks, similar as in [6],: wherein the distance between the centers of each circle of obstacles and the ego vehicle is required to be greater than the sum of each circle's radius. When the requirements are violated, f col becomes infinite. In Fig.  8(a), the resampled trajectory is shown, and in Fig. 8(b), the initial trajectory is optimized using the PSO algorithm, which generates a kinodynamically feasible, smooth, and safe trajectory.

V. VEHICLE-IN-THE-LOOP SIMULATION
The VILS [42], [43] is implemented on the TS KATRI PG track, as shown in Fig. 9 with virtual scenarios and a test vehicle equipped with GPS, wherein it is assumed that information regarding virtual surrounding obstacles' positions and velocities is perfectly known. The proposed hierarchical motion planning framework is implemented in C++ language on a PC running Ubuntu 16.04 equipped with a quad-core Intel Core i7-6700K CPU. The execution period of the algorithm was 100ms. For validation, five highway scenarios in a unidirectional road were generated. Our first four scenarios included static and dynamic obstacles modelled by intelligent driver model (IDM). The last scenario included an obstacle performing a sudden cut-in maneuver. These scenarios were selected to show that our algorithm can perform many functionalities such as lane changing, car-following, obstacle avoidance, and stopping in various speed ranges. The motion planner parameters for implementation are listed in Table 2.

A. TRAFFIC GENERATION
The obstacle vehicles are controlled by an IDM. To imitate human drivers who might be aggressive or defensive, both types of vehicles are included in the environment for the first two scenarios. Only one type of vehicle was considered for test scenarios 3 and 4. The dynamics of the vehicle are described by the following equations: where s * (v, ∆v) = s 0 +vT +v∆v/2 √ ab, v 0 , s 0 , T , a and b are the model parameters. The parameters of each vehicle are listed in Table 3.

1) Scenario 1 (Fast Dynamic Obstacle Scenario)
In this scenario, dynamic obstacles are set to have relatively high-speed profiles using the IDM parameters as shown in Table 3. A total of 10 dynamic obstacles were arbitrarily positioned on two-lane roads within a certain distance in front of the ego vehicle. The ego vehicle is expected to perform appropriate lane change and car-following maneuvers, generating a safe and smooth spatiotemporal trajectory. In Fig. 10(a), the upper figure shows the results of the motionplanning algorithm. The long-term path (blue) is the output of the first part of the algorithm, and the short-term trajectory (light blue) is the result of the last part of the algorithm. The bottom left figure shows the result of the JPS algorithm when solving the problem of the second part of the algorithm in the distance-time plane by displaying the planned trajectory and jump point. The bottom-right figure shows the velocity profile generated by the algorithm. The reference velocity was set to 20 m/s. The long-term profile (blue) is from the second part of the algorithm, and the short-term profile (light blue) is from the last part of the algorithm. In Fig. 10(a), the ego vehicle undergoes a lane change while accelerating to the reference speed. Next, in Fig. 10(b), the ego vehicle keeps the lane while maintaining the reference speed. Finally, in Fig. 10(c), the ego vehicle slows down while performing a car-following maneuver. There are two obstacles in the right lane, therefore a lane maintenance path is generated for the long-term path. Finally, the results in Fig. 10(d) show both the reference velocity and velocity profile of the ego vehicle during the entire scenario. In Fig. 11, the plots show the trajectories of the ego vehicle and obstacles within a certain time gap. The blue trajectory depicted with a triangle inside

2) Scenario 2 (Slow Dynamic Obstacle Scenario)
In this scenario, dynamic obstacles are set to have relatively low-speed profiles, as shown in Table 3. This scenario aims to evaluate the lane change and car-following performance in low-speed traffic. In Fig. 12(a), a long-term path is generated  to change the lane considering the surrounding obstacles. As shown in Fig. 12(b), car-following is performed because the risk of a lane change is high owing to the obstacle behind in the right lane. Then in Fig. 12(c), the ego vehicle performs a safe lane change maneuver as there is sufficient space. In the JPS plot in Fig. 12(c), while changing the lane and avoiding collision with obstacles, the planned trajectory shows that the ego vehicle tries to accelerate to the reference velocity. The overall maneuvering is shown in Fig. 13.

3) Scenario 3 (Static Obstacle Scenario)
In this scenario, static obstacles are placed in front of the ego vehicle. This scenario aims to evaluate the obstacle avoidance and stopping performance of the proposed algorithm. In Fig.  14(a), the ego vehicle avoids collision with a static obstacle  ahead by choosing a long-term path for lane changing. As can be seen in the JPS plot, an optimal long-term trajectory is obtained which decelerates through the optimal long-term path while keeping it collision-free. In Fig. 14(b) and (c), it can be seen that the ego vehicle continuously decelerates and finally stops to avoid collision with the obstacles. The overall maneuvering is shown in Fig. 15.

4) Scenario 4 (Hazard Dynamic Obstacle Scenario)
In this scenario, the hazard dynamic obstacles overlap with the lane. As shown in Fig. 16(a), the ego vehicle first changes lanes. Then, then, the long-term trajectory is generated as if there are no obstacles, as shown in Fig. 16(b). However, the short-term trajectory is generated considering the obstacles with collision cost described in Section IV-C, resulting in a trajectory that deviates from the reference trajectory. Next, as shown in Fig. 16(c), a short-term trajectory is generated toward the centerline when the collision risk disappears. The overall maneuvering is shown in Fig. 17.

5) Scenario 5 (Cut-in Scenario)
In this last scenario, a dynamic obstacle performs an abrupt cut-in maneuver in terms of duration and relative speed [44]. The obstacle performs a lane change for 3 s, the relative speed with the ego vehicle is -5 m/s, and the lane change starts 25 m ahead. This scenario is intended to see how the planner reacts to a lane change of the obstacle. In Fig. 18(a), the ego vehicle maintains the lane while avoiding collision, as shown in the JPS plot. Then, in Fig. 18(b), the ego vehicle avoids collision with the cut-in obstacle ahead by choosing a long-term path for lane changing. The ego vehicle successfully performs a lane change to the left, as shown in Fig. 18(c). The overall maneuvering is illustrated in Fig. 19.
The steering wheel angle of the ego vehicle for all test scenarios is shown in Fig. 20. The results show that appropriate steering wheel angle is applied to the ego vehicle in order to perform lane change maneuvers. In the test scenario 5, it is seen that an abrupt lane change is performed by applying a larger steering wheel angle than other scenarios to avoid collision with the cut-in obstacle. Remark 1. The selected five scenarios include representative situations that may be encountered while driving on a highway. There are static and dynamic obstacles, including hazard vehicles that deviate from the center of the lane. The autonomous vehicle performs lane keeping, following, lane changing, stopping and swerve for safe highway driving. Remark 2. In the test scenarios, the long-term path planner provides a desired maneuver with a lane-level lateral targets considering the lanes and obstacles. The paths generated show appropriate planning strategy, especially in lane selection. The long-term trajectory planner then applies the position and velocity profiles to the optimal path to avoid collisions with obstacles determined to overlap the path. Experimental results show satisfactory performance covering all scenarios. Remark 3. The last short-term trajectory planner optimizes the trajectory considering comfort and safety. The optimization using the particles of steering angle sets with an adequate vehicle model makes smooth and feasible trajectory. In addition, the obstacles not considered in long-term planner are considered in optimization procedure to make collision free trajectory. Fig. 21 shows a histogram of the computation time of the proposed algorithm for all scenarios. The distribution of the computation time is expressed through a histogram with an additional normal distribution. The average computation time was 31 ms, and the worst-case computation time was 94 ms.

VI. CONCLUSION AND FUTURE WORK
In this paper, the proposed hierarchical motion planning framework was shown to be a safe and efficient solution for autonomous driving in structured environments with various static and dynamic obstacles. The proposal was evaluated by means of a VILS with a real car under five representative scenarios.
The long-term planner, which consists of two processes, first provides a rough path for the driving strategy over a long time horizon. Then, the JPS is utilized to reduce the computational burden of A*, giving an acceptable quality solution. The generated trajectory was safe, comfortable, and dynamically feasible. This long horizon planner gives a long-sighted solution which can handle various maneuvers in decision-making where a plenty of obstacles exist.
Next, the short-term planner optimizes the front part of the rough trajectory by first resampling and then applying the PSO. This procedure provides kinodynamically feasible trajectory, enhancing the quality of trajectory in terms of comfort, smoothness and safety. The performance is evaluated under five scenarios, which include virtual static and dynamic obstacles. The proposed method provides a feasible and reasonable path and trajectory for performing lane changing, car-following, obstacle avoidance, and stopping in an appropriate manner. Furthermore, the computation time of the algorithm was within 100 ms.
As future work, the proposed planning framework will be tested on a broader range of traffic scenarios including urban situations, to further verify and evaluate the performance. The traffic light scenario can be conducted where a long horizon capable planning is important in maneuver decisionmaking (to pass or to stop at traffic light). In addition, to meet the requirements of advanced prediction methods [45]- [48], interaction-aware trajectory prediction with uncertainty could be considered in motion planning instead of simple physical model-based predictions. Finally, it could be interesting to focus on applying the deep learning technique to make human-like behavior decision-making.