Technical Feature

Hayoung Kim, Dongchan Kim, and Kunsoo Huh Hanyang University

Intention Aware Motion Planning with Model Predictive Control in ABSTRACT

Highway Merge Scenario Human drivers navigate by continuously predicting the intent of road users and interacting with them. For safe autonomous driving, research about predicting future trajectory of vehicles and motion planning based on these predictions has drawn attention in recent years. Most of these studies, however, did not take into account driver’s intentions or any interdependence with other vehicles. In order to drive safely in real complex driving situations, it is essential to plan a path based on other driver’s intentions and simultaneously to estimate the intentions of other road user with different characteristics as human drivers do. We aim to tackle the above challenges on highway merge scenario where the intention of other road users should be understood. In this study, we propose an intention aware motion planning method using finite state machine and model predictive control without any FIGURE 1: Highway merging scenario that requires other road user’s intention. vehicle-to-vehicle (V2V) or vehicle-toinfrastructure (V2I) communications. The Introduction Next, there exist studies which consider key idea is to design the behavioral planner There has been an active research to solve merging problem. First, there exist methods using Model Predictive Control (MPC) [1,2,3]. Mukai et al. [1] uses MPC to solve merging problem in highway scenario. Here, constant velocity model is used to predict the surrounding vehicle. Therefore, it is not realistic to apply in complex traffic situations. MPC is also used in [2]. In this work, in order to represent uncertainty of the physical state of the surrounding vehicle, Gaussian distribution is utilized. Zhan et al. [3] combines search-based method and optimization-based method to roughly plan longitudinal trajectory. Then, perform longitudinal and lateral trajectory smoothing using MPC. Studies in [1,2,3] have a limit in that they do not consider intention and interaction of nearby vehicles in common. intention of the surrounding vehicles and interaction with the host vehicle [4,5,6]. Milanés et al. [4] presented Fuzzy logicbased method to solve merging problem. They used physical state information which includes intention of the surrounding vehicles. However, there exists a limitation that the information used is obtained through vehicleto-infrastructure (V2I) communication. In the work [5], to model interaction with the surrounding vehicles, extended probabilistic IDM model is used. Here, trajectory prediction is performed estimating the parameters of the proposed IDM model. In the case of motion planning, the computation time is reduced by treating only longitudinal movements except the lateral movement through the road network represented by the arc length of 1-D. On the other hand, as can be seen in studies [1,7,8], in parallel lane merging scenarios where merge points are not defined, it is difficult to cope without considering lateral that control the possible modes like human drivers do. This behavioral planner contains “negotiate” state which could inform my intent to other road users and estimate the other user’s intention from their reaction. The model predictive controller generates an optimized trajectory for merging in terms of safety, efficiency and comfort with directly reflecting the estimated intention of the road users. In order to verify the proposed framework, the complex highway merging scenario is implemented where various road users with different intention and characteristic exist by using IDM (Intelligent Driver Model). planning. Hubmann et al. [6] deals with the merging scenario in the congested traffic situations. They use Partially Observable Markov Decision Process (POMDP) to model uncertain cooperation with the surrounding vehicles. In addition, Monte Carlo sampling algorithm is used to solve the combined longitudinal and lateral optimization problem. In this study, we propose an intention aware motion planning method using finite state machine and model predictive control in highway merge scenario. The proposed FIGURE 2: Overall architecture for highway merging. behavioral planner contains “negotiate” state

which could inform my intent to other road users and estimate the other user’s intention simultaneously.

System Architecture

The system for safely merging into highway consists of three modules as shown in figure 2. The future trajectory predictor predicts the other road participant’s future trajectory by using road structures and the tracking information from sensor fusion module. It differs from the existing methods in that it predicts the intention of the other vehicle and TABLE 1 : Target lane and target speed for each state in finite state machine. The value of target lane is relative: see Figure 4. simultaneously reflects it in the predictions. In the highway merging scenarios, it helps However, in a general situation if you do follows: First, in the “ready” state, it is first the host vehicle determine its behavior by not show your intention to merge into determined whether the vehicle is in the considering whether or not the other vehicle the highway first it is difficult to know the merging area or approaching the merging will yield to the host vehicle. intention of the driver in the other vehicle. This area. If we have a vehicle in the merging The behavioral planner sets the behavioral goals that the host vehicle should perform. The surrounding vehicle’s states, road network and intention prediction are used together in this module to set up the target lane and target speed profile for trajectory planner. It contains “negotiation” behavior to estimate the other road user’s intention. The trajectory planner calculates the collision-free trajectory by using the target lane and target speed profile from behavioral planner. It determines drivable space by using future trajectory predictions. After drivable space are set, jerkminimizing trajectories are calculated within this drivable space. If inevitably a collide is anticipated, it can be communicated to the behavioral planner so that the behavioral goal can be modified. This module also determines the steering angle and target acceleration for the vehicle to follow the trajectory. intention aware behavioral planning part will show you how to show your intent to others on the highway, determine if the opponent is making concessions, and then set the goal of the host vehicle. Finite State Machine for Behavior Planning Behavioral planning plays a role in establishing a strategy for vehicles to join into the highway road by considering surrounding traffic and road network information. It is important to drive naturally as human driver’s do for successful highway merging. For this reason, this behavior planning module is designed as a Finite State Machine (FSM), focusing on how people operate when merging onto a highway without crashing. The actions that vehicles can take in joining on the highway are limited to the states defined in the finite state machine. These states are composed of five states: “ready”, area and there is traffic on the highway, we “negotiate” with other traffic to determine if we can join. Rather than merging into the highway, the host vehicle approaches the lane mark drawn between the main lane and the current lane to judge whether another vehicle intends to give way to me. This process serves as turn signals. If the car on highway does not have the intention to yield to the host vehicle, the host vehicle will quickly get off the lane mark and “yield” the road to the car on highway. On the other hand, if the car in the highway decides to yield, the host vehicle will “merge” to the highway. On the other hand, if the car in the highway judges that it intends to give it to me, it will “merge”. If, during the “merge” process, a collision is predicted in the future, it will return to the “negotiation” state again. However, if it is judged that a “merge” is possible without a collision, the vehicle enters the space between the vehicle in front of the highway and the Intention Aware Behavioral Planning “negotiate”, “yield”, “merge”, and “cruise”. The vehicle behind it. When you successfully For a vehicle to safely merge into the highway, it is very important to estimate the driver’s intentions and set proper behavioral goal based on them. The success rate of merging is highly depending on accuracy of the intention estimation in dense highway situation. However, the intention is not observable from the environmental sensor. Therefore, it is challenging to estimate the possible transition in each state is defined as directed acyclic graph (DAG) as shown in figure 3. The decision process traverse from the “ready” state to the “cruise” state along the graph. In each state, it is necessary to define in advance what actions the vehicle should take and what conditions must be met to move on to the next state. For every state, the behavioral goals are shown in Table. 1. “merge” to the highway, the decision process is terminated by “cruising” the distance to the preceding vehicle. In the following part we will discuss in detail the role of the “negotiate” state defined in the state machine and how to make joining decisions. “Negotiate” State in Directed Acyclic Graph intention of the other road participants and Possible scenarios for merging into the The negotiation state determines whether important to planning process. highway based on this state machine are as to merge into the highway or to send a behind vehicle when there is a host vehicle in the merging area as shown in Figure 4. To make the decision, the Negotiation module considers two major things. The first is to identify the intention, I, of the behind vehicle and the second is to calculate the minimum safe distance, d safe . The intention of the behind vehicle can be predicted when the host vehicle approaches to lane marking between the lanes to reveal the intent of merging. If the behind vehicle is willing to yield, it will slow down when the host vehicle approaches the lane. On the other hand, if the behind vehicle does not intend to FIGURE 3: Finite state machine design for highway merging scenario. yield, it can be expected that it will move at

faster speed or maintain its speed. of the behind vehicle is used: V = { v 1 , v 2 , … , v N } where N is the number of elements is constant over the history interval, N, the concession intent can be determined by the tendency of the speed. Let the tendency of the speed be slope term p 1 for linear relations: p 0 T using least square method on speed profile V as follows: In this paper, we classify the intention of the behind vehicle to be “yield” when p 1 is negative. In order to merge to the lane without a collision, it is essential to consider the current speed and acceleration of the behind vehicle such that the distance is sufficiently large when the host vehicle merge in front of the behind car. This distance must be unconditionally secured and independent of the intent. For example, the safe distance, d safe , in [9] can be simplified for convenience: where v behind , ρ, a max , aceel , a min , brake are velocity of behind vehicle, response time, acceleration of behind vehicle and proper deceleration of behind vehicle, respectively, and v host , a max , brake are velocity of the host vehicle, maximum deceleration of the host vehicle, respectively. The value of proper deceleration a min , brake should be set considering other road user’s comfort. The larger this value, the greater the vehicle must decelerate when the host vehicle merge. Finally, in the “Negotiate” state, we decide whether to move to the “merge” state or the “yield” state, taking into account both the intention I of the behind vehicle and the safe distance d safe . When the distance is greater than the safe distance and the intention of the behind vehicle is determined to be concession, it goes to the “merge” state. This part covers how to use the information provided by the behavioral planner to generate longitudinal and lateral trajectory for real driving.

Longitudinal Trajectory Planning

Trajectory planning in longitudinal direction is divided into two cases according to the situation. We use a similar approach as [9] and generate trajectory for velocity keeping and distance keeping in a different manner. Then, various trajectory candidates are generated and the optimal one is selected.

Velocity Keeping Trajectory

First, as can be seen in Table 1., when the value of target lane is 0 or 0.5 and the host vehicle is in “ready”, “negotiate”, “yield” state, trajectory for velocity keeping is required. Here, quartic polynomial in the direction of the main road is used as follows: In order to solve the polynomial function, three initial states and two final states are used for boundary conditions. Here, to make various trajectory candidates, final conditions are varied by different Δ s m from the target velocity s target

FIGURE 4: Definitions for target lane, merging area and the behind vehicle, the ahead vehicles.

To estimate the intention, I, the speed history in speed history. If the intention, I, to yield + kp 1 = v k . Then, we can calculate P = [ p 0 p 1 ]

Trajectory Planning and Control Method

and time interval T n according to

Distance Keeping Trajectory

Next, as explained in Table 1., when the target lane is 1 and the host vehicle is “merge” or “cruise” state, distance keeping trajectory is needed. Here, quintic polynomial is used as follows:

To solve the polynomial function in this case, three initial states and three final states are used as boundary conditions. Here, to make various trajectory candidates, the deviation term Δ s m from the target position s target and time interval T are varied according to

Optimal Trajectory Selection

Depending on the situations of the behavior state, various trajectory candidates are generated in the manner described above. Here, in order to select the optimal trajectory, jerk, deviation from the target velocity and the time interval are considered in the cost function as follows:

where c jerk , c vel , c T are weights for each index. The trajectory candidate which minimizes the cost function is chosen as the optimal one. Then, target acceleration is calculated and it is transmitted to the lower level controller part. In addition, the velocity profile is transmitted to the lateral trajectory planning part so that it can be used when model predictive control is

performed.

Lateral Trajectory Planning Lateral Kinematic Model

The state equation of the vehicle on the vehicle coordinate can be expressed as below: On the other hand, in the highway merge situation, cos(ψ) ≅ 1, sin(ψ) ≅ ψ can be assumed because the heading angle has a small value when the vehicle is in control. In addition, as longitudinal and lateral trajectory planning are performed separately in this work, the longitudinal state ζ lon and the In summary, the state space equation about below: The kinematic model used for lateral planning is not well suited for slip situations. Nonetheless, the kinematic model is used rather than using the dynamic model in this study. The first reason is that kinematic model is easier at model identification. Then, for the second reason, in highway merge application where lateral acceleration a y is not large, it is experimentally better than dynamic model if mode validity is guaranteed. [11]

Model Predictive Lateral Trajectory Planning

For lateral trajectory planning, MPC method is applied. First, the lateral trajectory planner is designed by defining the constraints for stable trajectory planning without collision and defining the costs so that the target lane can be followed well within the constraints. Constraints. Lateral constraints consist of 1) safety constraints, 2) model validity constraint, 3) actuator limit constraint.

In the case of the safety constraint, as shown in Fig. 5, it makes the host vehicle drive safely in the drivable space using the predicted trajectory of the surrounding vehicle and the road boundary information. Here, slack variable ϵ is introduced in case the feasible trajectory cannot be found [12]. The equation is used as follows: For the validity of the kinematic model, lateral acceleration a y must meet the following conditions [13]. On the other hand, lateral acceleration can be expressed as a y = ÿ + ψ v x , in highway situation, assuming that ÿ ≅ 0, a y can be approximated as ψ v x . Also, yawrate ψ ≅(ψ (k be shown as follows: By multiplying the longitudinal velocity v x = derived: Therefore, the constraint on model validity can be changed from the lateral acceleration constraints to the control variable constraint with δ according to In the case of the actuator limit, constraints can be expresses as below considering the value of the steering angle and the variation of the steering actuator limits. Optimization Problem Formulation. The optimization problem for lateral trajectory planning is defined as below. The cost function is design as follows to create a trajectory that follows the target lane reference y ref and heading reference ψ ref

lateral state ζ lat = [y, ψ] T are dealt separately. the lateral state ζ lat can be expressed as

+ 1) - ψ (k))/ dt from the kinematic model can

v(k) on both sides, the following equation is

well within the constraints defined above. We formulate the optimization problem with safety, model validity, actuator limit constraints and kinematic model. The lateral trajectory planning problem is formed as following quadratic programming problem:

Simulation and Result

To evaluate the intention aware behavior planning and trajectory planning framework, two different type of road users (aggressive driver and defensive driver) based on intelligent driver model (IDM) are used in scenarios. The simulation environments are based on real test road. The proposed framework is implemented in C++ and python with Robot Operating System (ROS). To solve model predictive control problem, CVXGEN [14] is used, which provides an optimal solution in several milliseconds. The simulation environments are conducted by using Unity 3d, which is the real-time engine for game and simulation.

Traffic Generation

Traffic was generated using the Intelligent Driver Model (IDM) to generate road users with various intentions. Simulator randomly generates aggressive driver and defensive driver. The parameters of each are shown in Table 2. It moves with the following dynamics according to each parameter:

Results

To evaluate the proposed framework for highway merging, randomly created scenarios are conducted. There were two representative decision-making process depending on other road user’s intention. First, host vehicle tried to negotiate for merging onto the highway approaching to target lane and the other road users didn’t give a way. Figure 6 shows the negotiating process, where the behind vehicle’s intention is “not yield”. The host vehicle entered merging area after t = 5(s). When the car entered merging area, the car tried to nudge the behind car to “yield”. As the behind car didn’t slow down the speed, the host vehicle decided to “yield” so that the behind vehicle can pass the host vehicle. Even though the host vehicle gives a way to the other vehicle, as the another behind vehicle take a way, the host vehicle should slow down to wait another chance to merge. Figure 7 shows the decision-making process which successfully traversing directed acyclic graph from “ready” to “cruise”. It shows the situation when the behind vehicle slows down to make room to merge for the host vehicle. After t = 8.5 (s), the host vehicle starts to merge and finally at t = 10.5 (s), the host vehicle successfully merges onto the highway.

FIGURE 6:

A first example sequence of highway merging scenario. Red colored vehicle is host vehicle and blue colored vehicles are other road users. The red line and the blue line represent lane before the merge and lane after the merge respectively. The current behavior is written on the title of each figure. FIGURE 7: A second example sequence for successfully merging into the highway.

Conclusions

In this study, an intention aware motion planning framework is presented using finite state machine and model predictive control method in highway merging scenarios. The behavioral planner is designed to guess the other road user’s intention, which referred as “negotiate” state. In addition, to achieve the behavioral goal without collision, the longitudinal trajectory planner and the lateral trajectory planner are designed. Especially, by separating the longitudinal trajectory planner from the lateral trajectory planner, the lateral trajectory planning problem is simplified as quadratic programming. The simulation results demonstrate that the proposed framework could work well in highway merging scenario with randomly sampled drivers based on intelligent driver model. Future work will focus on how to cope with the uncertainty on sensor for robustness of algorithm.

Acknowledgement

This research was supported by Korea Evaluation Institute of Industrial Technology(KEIT) grant funded by the Korea government(MOTIE) (No. 10052375, Test Scenario Study for Supporting Autonomous Driving Technology Development).