How Shall I Drive? Interaction Modeling and Motion Planning towards Empathetic and Socially-Graceful Driving

01/28/2019 ∙ by Yi Ren, et al. ∙ Arizona State University 0

While intelligence of autonomous vehicles (AVs) has significantly advanced in recent years, accidents involving AVs suggest that these autonomous systems lack gracefulness in driving when interacting with human drivers. In the setting of a two-player game, we propose model predictive control based on social gracefulness, which is measured by the discrepancy between the actions taken by the AV and those that could have been taken in favor of the human driver. We define social awareness as the ability of an agent to infer such favorable actions based on knowledge about the other agent's intent, and further show that empathy, i.e., the ability to understand others' intent by simultaneously inferring others' understanding of the agent's self intent, is critical to successful intent inference. Lastly, through an intersection case, we show that the proposed gracefulness objective allows an AV to learn more sophisticated behavior, such as passive-aggressive motions that gently force the other agent to yield.



There are no comments yet.


page 2

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

While intelligence of autonomous vehicles (AVs) has significantly advanced in recent years, accidents involving AVs suggested that these autonomous systems lack gracefulness in driving. In one incident, Waymo reported that its AV (denoted by “M” hereafter) was stuck at an intersection because human drivers (denoted by “H”) coming from the other road never stopped fully while approaching the intersection, making M believe that H wanted the right of way and causing it to stop indefinitely [1]. The first claim against an AV manufacturer has been filed against General Motors in 2018, following a collision between a Cruise Automation 2016 Chevrolet Bolt and a motorcyclist, who alleged that the car swerved into his path without his agreement [2].

Both incidents can be attributed to M’s lack of understanding of H’s intent and thus its failure to predict H’s future motion. In interactions that involve both agents as decision makers, M’s inference of H’s intent depends on not only observations from H, but also knowledge about H’s understanding of M. In other words, M needs to be empathetic in order to understand what leads to H’s behavior. To elaborate, consider that an agent’s intent is defined as parameters of its control objective. The interaction between M and H from the perspective of M can be modeled as results from a series of games along time, with each new game parameterized by M’s intent, and M’s current inference of H’s intent. M chooses motion based on the perceived game and its planning strategy (e.g., reactive or proactive planning, see Sec. IV for definitions), and assumes that H follows the same decision making process for motion planning, with its own perception of the game and planning strategy. Under this game-theoretic setting, the inference of H’s intent is in conjunction with that of H’s inference of M’s intent, i.e., what H expects M to do, which is in general different from M’s true intent. Enabling M’s awareness of the difference between M’s and H’s perception of the interaction is what makes M empathetic.

Building upon this, this paper further introduces a motion planning strategy that incorporates social gracefulness, which is defined as the discrepancy between the actions taken by M and those that could have been taken in favor of H. Take the intersection case in Fig. 1 for example. When H moves slowly, an empathetic M knows that he is expected to take the right of way. A socially-aware M, in addition, knows that himself driving slowly to allow H to pass would do H a favor. Note that to acquire social awareness, M would need to understand H’s intent, and thus be empathetic in the first place. We will show that considering the gracefulness objective in planning allows M to learn more sophisticated behavior, such as passive aggressive motions that gently force H to yield.

Fig. 1: An illustration of social awareness of an agent. By inferring H’s intent, M infers how H will move, how M is expected to move, and what movements of M will be in favor of H.
Fig. 2: Summary of the intent inference and motion planning algorithms of M. To perform intent inference, M needs to model H’s motion planning hypothetically. The highlighted modeling elements include (1) the game-theoretic intent inference algorithm that enables empathy, (2) the incorporation of inference uncertainty in motion planning, and (3) more sophisticated motion planning objectives enabled by intent inference.

The technical approaches of the paper can be summarized in two steps: In Sec. III, we formulate the intent inference problem where the intents of both agents are jointly inferred, based on the assumption that the both agents model each other to adopt a baseline planning strategy where the agent plans motions based on the set of Nash equilibria of the game perceived by the others. In Sec. IV, we explore more sophisticated planning strategies enabled by the inferred intents, and evaluate how these strategies influence the actual social gracefulness and social efficiency of an interaction between H and M. Three driving strategies will be explored: (1) A reactive M plans based on the inferred distribution of future motions of H. (2) A proactive M exploits the fact that H’s future motion are dependent on M’s. (3) A socially-aware M is built upon the proactive model, with the additional gracefulness loss. The proposed intent inference and motion planning models are summarized in Fig. 2.

The intellectual contributions of this paper are two-fold: First, we propose mechanistic intent inference and motion planning algorithms for two-agent interactions. Our approach is different from existing studies where the control policy or objective of either M or H is learned offline through data. The introduction of empathy was previously discussed in Liu et al. [12]. This idea is extended to non-collaborative scenarios with non-quadratic control objectives. Second, we introduce the definition of social gracefulness as a performance measure for human-machine interactions. The idea of measuring risk through the difference between the driver’s actions and what he is expected to do according to traffic rules was introduced in [7]. We take a parallel approach by defining the nominal actions as shaped by others’ intents (i.e., tacit social norms), rather than by explicit traffic rules.

Ii Related Work

AV has been an active and fast developing research topic in the past few years. Scenarios such as lane changing, lane merging, and traffic intersections, where AVs closely interact with human-driving vehicles, are widely investigated [11, 21, 8]

. Human drivers are often modeled as rational and intelligent decision makers in these studies. Pentland and Liu proposed a dynamic Markov models to recognize and predict human driving actions 


. Levine et al. adopted an inverse reinforcement learning method to explore human drivers’ reward functions based on human demonstration recorded through a driving simulator 


. Machine learning is used to predict the lateral motions and braking actions of human driver 

[19, 14, 17]. These data-driven approaches require a large amount of data for model training. Wang et al. discussed the appropriate amount of data to achieve sufficient understanding of human driving behavior [18].

Based on proper human driver models, an essential question remains as how to achieve safe and efficient human-machine interactions. Nikolaidis et al. [13] proposed a mutual adaptation algorithm where human’s willingness to adapt is inferred and incorporated into the robot’s motion planning. In a similar two-agent collaborative setting, Liu et al. [12] proposed a “Blame-all strategy” for intent inference. Much like this paper, “Blame-all” performs inference of others’ intents by taking into account others’ inference of the agent’s intent, with the limited scope of quadratic control objectives. For dynamically changing agent parameters (such as intents), Foerster et al. [5] proposed Learning with Opponent-Learning Awareness (LOLA), a method for agents to incorporate the anticipated learning of other agents during its own adaptation (similar to [16] and the proactive vs. reactive case of this paper). LOLA, however, had limited discussion on scenarios where opponent parameters and parameter tuning schemes need to be inferred in a higher level game (e.g., proactive vs. proactive as in this paper). Specific to AVs, various game theoretic models have been constructed to describe different interactions between human drivers and AVs [20, 3]

. In a setting of partially observable Markov decision process, Li et al. introduced a “Level-k” reasoning model to evolve the strategies that agents adopt in driving interactions 

[10]. Nonetheless, their solution requires an iterative search through reinforcement learning. In contrast, mechanistic models (e.g., [12, 13] and this paper) are one-shot and are more suitable for scenarios where agents are required to learn and adapt in real time.

Iii Intent Inference

This section introduces the motion planning game and the inverse problem of intent inference. For simplicity, we focus on an interaction between two agents, M and H. The extension to multi-agent settings will be left as future work. The inference by M is based on the assumptions that H uses a baseline motion planning strategy (see Sec. III-B), and that H believes that M uses the same strategy.

Iii-a The motion planning game

Let and be the state and action at time , with the subscript specifying the agent. We consider deterministic state transition: , and define ’s planned motion at as a sequence of actions , where is a predefined finite time horizon. The dependency on will be omitted for brevity. The planning game is formulated as follows: The instantaneous loss of agent is


where measures the safety loss, the fulfillment of a task, e.g., “lane changing” or “crossing the intersection” (see Sec. V), and the intent parameter governs the behavior of the vehicle. Note that involves the distance between and , and thus requires , , , and as inputs. The dependency on and will be omitted for brevity. The payoff for in the game is defined as the accumulative loss


for that satisfies , which represents boundaries on future states. For infeasible , . Each agent chooses its motion from a finite candidate set . Under this setting, the game is guaranteed to have at least one Nash equilibrium (Theorem 23 in [6]).

Denote as

’s estimation of

’s intent (see Sec. III-C for the inference of ). The game at time yields a Nash equilibrium set , where is ’s estimation of ’s planned motion. Each element of satisfies


For example, at an intersection without a stop sign, either M or H yielding to the other will satisfy Eq. (3) when both M and H are non-aggressive (i.e., with small ). It is important to note that for to understand , needs to put himself in the shoes of and derive the equilibrium set from ’s perspective. Since does not know , a necessary correction is to replace with , i.e., ’s estimation of ’s estimation of ’s intent, and similarly, with .

Iii-B The baseline driving strategy

Agent is modeled to believe that plans its motion at by sampling uniformly from , i.e.,

’s motion follows the probability mass function:

where is the cardinality of a set.

Iii-C Inference of intent and motion

Eq. (4) formulates the inference problem where we estimate the intents ( and ) and the planned motions ( and ) at time based on the states and actions of and at . The idea is that the estimated intents should be such that ’s planned motion at with the highest probability mass (denoted by ) should have its first action (denoted by ) matching the observed action of at (), or verbally, what should have done should match with what it actually did.

subject to

We use a finite intent set for both agents to represent different levels of aggressiveness in driving. Since both and are finite, the inference is done through an enumeration over the solution space . The outcome of the enumeration is a set , where each element is a global solution to Eq. (4). To quantify the uncertainty in inference and later incorporate it into motion planning, we assign equal probability mass (

) to each solution, based on which we can compute the empirical joint distribution

and the marginals and , by counting the appearances of all elements of in . From the joint distribution , we can infer ’s planned motion. From each we can compute a conditional distribution of ’s motion starting from , denoted by , through . We can then calculate the marginal using and . Note that this is the distribution of ’s motion at rather than since we formulated the game at . Although one can formulate a new game at to derive , in this paper we will approximate using . Similarly, we can also infer ’s planned motion as expected by , denoted by . We shall emphasize that is not the distribution of motion that will follow, rather, it represents ’s awareness of what expects it to do.

Iii-D Leveraging past observations

Note that if we consider to be time invariant during the interaction, then all previous observations can be leveraged to infer , leading to a modified problem:

subject to

We allow to freely change along time since may change its mind about . Solutions to Eq. (5), denoted by , can be found in a recursive way based on solutions to Eq. (4). Consider an intent candidate that exists in both and . Let solutions in that contain be in the form of , where is an array of intents of . Similarly, let solutions in that contain be in the form of , where is an intent of . Also denote the operation of appending element to the array a as . Then for all and is a solution to Eq. (5) at . Following this property, we have . The update of will trigger that of the joint probability and the marginal . Their updated counterparts are denoted by and , respectively.

Iv Motion Planning

We introduce three planning strategies that incorporate the inferred intents and motions. Note that in this paper, agents are only modeled to infer others’ intents rather than their planning strategies, i.e., they are assumed to use the baseline strategy but may in fact use different ones (see Fig. 2). Potential formulations of higher-level inference algorithms will be discussed in Sec. VI.

Iv-a Reactive motion

Given the distribution of ’s future motions , a reactive agent plans its motion by minimizing the expected loss within a time window:

Iv-B Proactive motion

A proactive agent plans by taking into consideration the dependency of ’s planning on ’s next action. Specifically, calculates the conditional distribution based on and , assuming that will quickly respond to . To do so, first finds the set of optimal motions of for every given . This set is denoted by where . Then for each element , we can compute

where is an indicator function. For , we set . We can now develop the planning problem for a proactive agent:

Iv-C Socially-aware motion

To develop the planning problem for a socially-aware agent , we will first find the set of motions that are wanted by at time . By definition, belongs to motion tuples that minimizes . This set is denoted by . Based on and , we can calculate the conditional probability and the marginal . A socially-aware agent solves the following problem where the difference between ’s planned motion and the wanted motion from is added to the proactive loss:

The weight tunes the importance of social awareness in the objective: A large will make follow what it believes that wants it to do, while a small will convert to a proactive agent.

V Simulation Cases and Analyses

In this section we will empirically answer two questions: (1) How do planning strategies influence interaction dynamics, and (2) why empathy is important for intent inference? We base all experiments on a simulated intersection case introduced as follows.

V-a Simulation setup

The intersection case is summarized in Fig. 1, where M moves up (along y-axis) and H moves left (along x-axis). Motion representation: With minor abuse of notation, we define as a scalar that determines the distance covered by an agent in steps, and assume that each agent moves at a uniform speed and a predefined direction: and . The candidate motion set is , where means backing up. Initial conditions: M and H are initially located at and . The initial motions are set to . Losses: penalizes the agent if it fails to move across the intersection within steps. Taking as an example, the loss is defined as , where is the state of M in the direction. The safety loss is defined as , where when both cars are in the interaction area as shown in Fig. 1, and otherwise ; so that the safety penalty increases significantly as the two cars approach each other; where is the length of the car. This setting creates a safe zone for an agent since the penalty diminishes when . For the socially-graceful agent, is set to 0.1 for the following experiments. A parametric study on will be discussed later in this section. Performance metrics: We measure the performance of a pair of motion planning strategies, denoted by for , using two metrics, namely, social gracefulness and social efficiency. Gracefulness, denoted by , is defined as

where is the time horizon for the entire simulation, is the action taken by M at using strategy , and is the action that H actually wants M to perform, which can be different from what M believes that H wants it to perform. We only measure gracefulness of M (the AV). Social efficiency, denoted by , is defined as the time required for the agents to reach an agreement. An agreement is reached at time when what expects to do matches with what actually does, i.e., . Therefore we can define

Experiment setup: The experiments will enumerate over and . represents a non-aggressive agent that will try to avoid being in with the other agent; an extremely aggressive agent that is willing to collide with the other agent. To test whether an agent can handle unexpected intents, the candidate intent set for decoding is set to . represents an aggressive agent that avoids collision. Conflict in inference: It is worth mentioning that since the agents do not follow the baseline algorithm for choosing a motion, their motions do not necessarily belong to an equilibrium set. This causes occasions where the probability mass of the true intent of becomes zero as inferred by

, or where the updated probability distribution, e.g.,

, has all zero entries when all candidate intents have been eliminated during the inference process. In the implementation, we set

back to a uniform distribution when the latter happens.

V-B Influence of planning strategy on the interaction

We use Table I to summarize gracefulness and efficiency under all combinations of planning strategies and intent settings. Due to page limit, we highlight two key findings. (1) The socially-aware strategy balances efficiency and gracefulness, provided that human drivers are reactive [4] and non-aggressive (). In particular, a socially-aware M takes advantage of a non-aggressive agent, and thus it makes the interaction more efficient than a reactive agent (which leads to a stagnation as seen in Fig. 3a), while at the same time in a more graceful way than a proactive agent (comparison between Fig. 3b and c). (2) In comparison with proactive planning, the socially-aware M avoids “panic reactions” when facing unexpectedly aggressive H during an interaction. As seen in Fig. 3d, a proactive M realizes that it needs to back off after it enters the interaction zone, forcing itself to make large change in its motion. In comparison, a socially-aware M avoids this as it has been partially complying to H’s intent all along the way, thus creating a smoother negotiation with H, see Fig. 3e.

reactive H proactive H social H
reactive M (, ) (, ) ()
proactive M (, ) (, ) (, )
social M (, ) (, ) (, )
reactive M (, ) (, ) (, )
proactive M (, ) (, ) (, )
social M (, ) (, ) (, )
TABLE I: and (, ), lower values are better)
Fig. 3: Snapshots of the interactions. (a) Reactive M vs. reactive H, . Both agents take the same actions at every time step since they share the same settings. After the initial action at , both take the smallest step at due to the concern that the other agent will take . This iteration between two action values continues indefinitely. This deadlock can be broken when the agents have significantly different s. (b) Proactive M vs. reactive H, . M successfully tricks H into believing that M is aggressive, and forces H to yield. (c) Graceful M vs. reactive H, . M reaches the interaction zone slightly before H, gently forcing H to yield. (d) Proactive M vs. reactive H, , . M reaches the interaction zone and realizes that it has to back off. (e) Graceful M vs. reactive H, , . M reaches the interaction zone after H due to its tendency to improve gracefulness, and therefore avoids sudden change in action. Legend follows Fig. 1.

V-C Parametric study on

We performed a parametric study on the gracefulness weight to examine its influence on the gracefulness and efficiency of the interaction. Results are summarized in Table II. In all cases, H is reactive and non-aggressive. As increases, M improves its gracefulness by taking actions closer to what is wanted by H. The efficiency, however, becomes worse towards as the interaction becomes similar to that of reaction vs. reaction. Verbally, being graceful but not enough graceful will cause confusion in interactions. As further increases, M converges to follow a motion that is in favor of H, and thus improves the efficiency.

right-of-way M M H H H H
TABLE II: Parametric study on the gracefulness weight

V-D The importance of empathy in intent inference

Fig. 4: (a) Interactions for the empathetic and non-empathetic cases. (b) Intent inference ( by M and by H) along time.

We now investigate the importance of empathy in intent inference using a similar experimental setting as above. We show that if agent is modeled to believe that agent already knows its true intent, then will create false estimation of ’s intent. Experiment setup: In the same intersection scenario, we use a reactive M vs. a reactive H setting with and . In the non-empathetic case, we fix for M’s inference processes, and keep H as an empathetic agent. Analysis: Fig. 4 summarizes the differences between the non-empathetic and empathetic cases. In the physical space, we observe that M in the non-empathetic case iterates between a high and a low speed, similar to the reactive vs. reactive case with , until when M realizes that H might be more aggressive than it thought, and starts to back off. In the empathetic case, this abrupt action change does not happen. Instead, M realizes the aggressiveness of H earlier, and chooses to stop before it enters the interaction zone.

This difference in the physical space is caused by that of the intent inference, see Fig. 4b. We will focus the discussion at where the two cases starts to depart from each other. At this time step, H takes and M chooses to stop for both cases. The equilibrium sets for all intent combinations are summarized in Table III. In the empathetic case, based on Eq. (4), we have since this is the only combination that explains . This leads to . For the non-empathetic case, since M believes that H knows about its non-aggressiveness, we have . This is because with , the only equilibrial motions of H are and , between which the former is closer to the observation. This misinterpretation led to , and M’s more advantageous motion in the following steps.

TABLE III: Equilibrium sets at , reactive M and H, ,

The above analysis showed that by fixing , the agent excludes plausible explanations of others’ behavior.

Vi Conclusion and Future Work

This paper investigated an empathetic intent inference algorithm for a non-collaborative interaction between two agents, and evaluated motion planning formulations that are enriched by the inference, with respect to social gracefulness and efficiency.

We now discuss limitations of the presented study and propose future directions.

The inconsistency between intent decoding and motion planning

One may notice that during motion planning, incorporates the uncertainty of the inference of ’s future motion. However, in intent decoding, assumes that uses a one-hot estimation of ’s intent () in its planning. To fix this inconsistency, would need to model to have considered a distribution of intents of in ’s planning, leading to an inference of the distribution of the distribution (of ) by .

Provable necessity and sufficiency of empathy

We demonstrated in Sec. V a case where non-empathetic inference leads to false estimation of others’ intent. However, we have not yet investigated the necessary and sufficient conditions under which empathy will be necessary for the inference to achieve correct convergence. Or mathematically, what are the necessary and sufficient conditions to have and for some ?

Inference of planning strategy

The above discussion suggests that due to the variation of planning strategies, intent inference at a parametric level may not be effective when one has a wrong guess of the others’ strategy. The question is then how the inference of strategies can be incorporated, for example to differentiate a proactive agent that pretends to be aggressive and an aggressive agent. In the discrete setting as presented in this paper, the inference can be done by enumerating over all candidate strategies. This, however, will not elegantly accommodate the estimation of hyper-parameters such as

. Another potential approach is to introduce a meta-objective as a weighted sum of loss functions collected from all strategies, and to infer the strategy by estimating the weights of this meta-objective, in addition to the intent parameters.

Computational challenges

Last but not least, extending the proposed decoding and planning algorithms to continuous domains will be necessary for their scalability in multi-vehicle coordination scenarios. However, doing so will introduce computational challenges due to the non-convexity of the inference and planning problems. In addition, the incorporation of a high-dimensional distribution of inferred motions in motion planning can be intractable. We expect scenario-specific solutions to these challenges.


  • [1] “Google is training self-driving cars to mimic human driving tendencies,”, accessed: 2018-07-25.
  • [2] E. Baron, “Blame game: Self-driving car crash highlights tricky legal question,” Jan 2018. [Online]. Available:
  • [3] D. Carlino, S. D. Boyles, and P. Stone, “Auction-based autonomous intersection management,” in 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013), pp. 529–534.
  • [4] M. A. Costa-Gomes, V. P. Crawford, and N. Iriberri, “Comparing models of strategic thinking in van huyck, battalio, and beil’s coordination games,” Journal of the European Economic Association, vol. 7, no. 2-3, pp. 365–376, 2009.
  • [5] J. N. Foerster, R. Y. Chen, M. Al-Shedivat, S. Whiteson, P. Abbeel, and I. Mordatch, “Learning with Opponent-Learning Awareness,” arXiv:1709.04326 [cs], Sep. 2017, arXiv: 1709.04326.
  • [6] A. X. Jiang and K. Leyton-Brown, “A tutorial on the proof of the existence of nash equilibria,” University of British Columbia Technical Report TR-2007-25. pdf, vol. 14, 2009.
  • [7] S. Lefèvre, C. Laugier, and J. Ibañez-Guzmán, “Intention-Aware Risk Estimation for General Traffic Situations, and Application to Intersection Safety,” INRIA, Research Report RR-8379, Oct. 2013.
  • [8] S. Lefèvre, D. Vasquez, and C. Laugier, “A survey on motion prediction and risk assessment for intelligent vehicles,” Robomech Journal, vol. 1, no. 1, p. 1, 2014.
  • [9] S. Levine, Z. Popovic, and V. Koltun, “Feature construction for inverse reinforcement learning,” in Advances in Neural Information Processing Systems, 2010, pp. 1342–1350.
  • [10] N. Li, D. W. Oyler, M. Zhang, Y. Yildiz, I. Kolmanovsky, and A. R. Girard, “Game theoretic modeling of driver and vehicle interactions for verification and validation of autonomous vehicle control systems,” IEEE Transactions on control systems technology, vol. 26, no. 5, pp. 1782–1797, 2018.
  • [11] C. Liu, C. W. Lin, S. Shiraishi, and M. Tomizuka, “Distributed conflict resolution for connected autonomous vehicles,” vol. 3, no. 1, pp. 18–29.
  • [12] C. Liu, W. Zhang, and M. Tomizuka, “Who to blame? learning and control strategies with information asymmetry,” in 2016 American Control Conference (ACC), pp. 4859–4864.
  • [13] S. Nikolaidis, D. Hsu, and S. Srinivasa, “Human-robot mutual adaptation in collaborative tasks: Models and experiments,” vol. 36, no. 5, pp. 618–634.
  • [14] K. Okamoto and P. Tsiotras, “A comparative study of data-driven human driver lateral control models,” in 2018 Annual American Control Conference (ACC).   IEEE, 2018, pp. 3988–3993.
  • [15] A. Pentland and A. Liu, “Modeling and prediction of human behavior,” Neural computation, vol. 11, no. 1, pp. 229–242, 1999.
  • [16] D. Sadigh, S. Sastry, S. A. Seshia, and A. D. Dragan, “Planning for Autonomous Cars that Leverage Effects on Human Actions,” in Robotics: Science and Systems XII.   Robotics: Science and Systems Foundation, 2016.
  • [17]

    J. Tang, F. Liu, W. Zhang, R. Ke, and Y. Zou, “Lane-changes prediction based on adaptive fuzzy neural network,”

    Expert Systems with Applications, vol. 91, pp. 452–463, 2018.
  • [18] W. Wang, C. Liu, and D. Zhao, “How much data are enough? a statistical approach with case study on longitudinal driving behavior,” IEEE Transactions on Intelligent Vehicles, vol. 2, no. 2, pp. 85–98, 2017.
  • [19] W. Wang, J. Xi, and D. Zhao, “Learning and Inferring a Driver’s Braking Action in Car-Following Scenarios,” IEEE Transactions on Vehicular Technology, vol. 67, no. 5, pp. 3887–3899, May 2018.
  • [20]

    H. Yu, H. E. Tseng, and R. Langari, “A human-like game theory-based controller for automatic lane changing,”

    Transportation Research Part C: Emerging Technologies, vol. 88, pp. 140–158, 2018.
  • [21] M. Zhang, N. Li, A. Girard, and I. Kolmanovsky, “A Finite State Machine Based Automated Driving Controller and its Stochastic Optimization,” no. 58288, p. V002T07A002, 2017.