Threat-aware path planning in uncertain urban environments

This paper considers the path planning problem for an autonomous vehicle in an urban environment populated with static obstacles and moving vehicles with uncertain intents. We propose a novel threat assessment module, consisting of an intention predictor and a threat assessor, which augments the host vehicle's path planner with a real-time threat value representing the risks posed by the estimated intentions of other vehicles. This new threat-aware planning approach is applied to the CL-RRT path planning framework, used by the MIT team in the 2007 DARPA Grand Challenge. The strengths of this approach are demonstrated through simulation and experiments performed in the RAVEN testbed facilities.


I. INTRODUCTION
Whether driving on highways or navigating in the middle of a battlefield, intelligent vehicles must be able to quickly and robustly compute motion plans in very uncertain worlds. The sources of this uncertainty may be internal, i.e., incomplete or imperfect knowledge of the vehicle model, or external, i.e., incomplete or imperfect knowledge of the environment, and may be present either in sensing or in predictability [1]. This paper addresses problems involving uncertainty in predictability, and in particular the intentions of the other vehicles within our vehicle's world.
To make meaningful predictions of other vehicles' intentions, a smart vehicle should be able to gather information from the environment to build models approximating those intentions. It is not safe to assume that all drivers obey all "rules of the road," as many collisions occur when this is not the case, but classifying all other vehicles as hostile would be overly conservative. The information gathered by our vehicle might include onboard camera images, radarbased measurements of surrounding objects, or messages intercepted or shared on some communication channels.
The vehicle planner must be able to address the uncertainty in each vehicle's future state and actions, or at a higher level, the behavior / intent governing those actions. Existing path planners typically rely on a priori information to generate trajectories, applying reactive maneuvers or replanning as needed to correct the path online and maintain feasibility. However, uncertainty in object predictability is typically not considered explicitly in the global path planner. Instead, moving vehicles are often assumed to follow known trajectories or policies, which the planner reacts to locally. While full probabilistic representation of the environment is possible, such as with POMDPs [2], [3], the corresponding solution techniques are computationally intractible for real-time path planning problems of even modest complexity or dimension. Recently, sampling-based techniques such as rapidlyexploring random trees (RRTs) [4] have been extended to incorporate motion patterns for moving obstacles learned offline via Gaussian processes [5]. However, these patterns may not be sufficient to capture all possible unexpected behaviors, which are usually the main reason behind collisions.
The main motivation of this work is the set of challenges faced by autonomous vehicles in the 2007 DARPA Grand Challenge (DGC) [6], which involved navigating an outdoor urban environment in the presence of other vehicles while obeying all traffic regulations. One of the main challenges of this race was negotiating traffic intersections, where several vehicles were involved in collisions or near-collisions. Several explanations have been offered for these occurrences, but the one that motivated this work is the inability of the autonomous vehicles to anticipate the intent of other vehicles [7]. With some knowledge of those intentions, the motion planner could incorporate the risk posed by those vehicles when considering potential trajectories, improving safety. This paper introduces a new framework for autonomous vehicles which enables path planning algorithms to explicitly incorporate both obstacle uncertainty and the corresponding risk posed to the vehicle. A threat assessment module, consisting of an intention predictor and a threat assessor, augments the host vehicle's path planner with a real-time threat value for each potential trajectory, reflecting the risks posed by the estimated intentions of other vehicles. The strengths of this approach are demonstrated through simulation and experiments performed in the MIT RAVEN facility [8].
II. PROBLEM STATEMENT Consider the autonomous system denoted by HV, where s is the state and u is the control input; we have that s is constrained to the state space S, s ∈ S, while u is contstrained to the control space U, u ∈ U. Our objective for safe motion planning is to minimize some desired cost functional over the duration of the motion, Of the many constraints C might include, we are interested in those necessitating collision avoidance with N other vehicles, denoted here as OV i , i = 1 . . . N . These can typically be written in the form where r i (t) is the state of vehicle i at time t, p s (t) is the position vector corresponding to s(t), p ri (t) is the position vector corresponding to r i (t), is the minimum allowed distance between the HV and the OVs, and t f = t 0 + T h where T h is the time horizon of interest. We say that HV is in collision with OV i if the tuple (p s (t), p ri (t)) enters the closed "collision" set Ω i , In uncertain and/or non-cooperative environments, r i (t) is typically not available for the HV. For HV to maintain guaranteed safety, some mechanism must be able to identify the input sequence u(t) which yields a feasible path for all possible realizations of r i (t), i = 1 . . . N . In the event that no such path exists, the same mechanism should select u(t) in order to minimize some threat level. Here we define the threat level T i for vehicle i as inversely proportional to t ci , the earliest possible time of collision between HV and OV i : Equation (4) thus cannot typically be guaranteed at all times, so this constraint is instead converted to a penalty in the objective function. We define the new cost functional where L is defined in (2), T i is the threat level defined in (6), and w is a user-selected weighting factor representing the tradeoff between safety and optimality. The constraints (4) are then removed from (3), yieldinḡ Problem Definition (Threat-Aware Dynamic Motion Planning): Given a state space S, a control space U, an initial state s 0 ∈ S and a final state s f ∈ S, compute the control input sequence u(t), t ∈ [0, t f ], t f ∈ [0, ∞) that minimizes (7) subject to the constraints (1) and (8).
III. THREAT-AWARE PATH PLANNING Several global path planners, including sampling-based planners, use the philosophy that collision detection should be done in a "black box" which decouples the host vehicle's path planning from any specific geometric or kinematic obstacle models [9]. This paper proposes that threat assessment should be done in a similar "black box" framework, providing planners with a quantitative threat metric to identify safer paths during trajectory generation. This threat assessment considers the unknown intentions of moving OVs, which may present a collision risk for the HV.

A. High-level TAM Architecture
The proposed architecture features a threat assessment module (TAM, Figure 1), consisting of an intention predictor (IP) and a threat assessor (TA). The IP uses observations taken by the HV of the OVs to make predictions of their intentions. Instead of using low-level reasoning to predict future trajectories, the IP uses higher-level logic and learning to provide the planner with predicted future intentions, key to earlier and more accurate prediction of future collisions. The TA then converts this prediction into a set of potential paths for the OV, and returns a threat value that a candidate trajectory u(t) (or equivalently, s(t)), t ∈ [t 0 , t f ] for HV incurs in the vicinity of these potential paths. As a result, the planner generates paths that are threat-aware by construction, thus improving the vehicle's reactive behavior.
Several distinct representations have been proposed to model intentions for humans and autonomous systems; here we adopt the Ecological Recogniser architecture introduced by [10]. Using the language of [10], the HV is the recognising agent, while each OV is an intending agent. We follow the abstract definition that these intentions are directly responsible for the actions executed by the intending agents. Intentions may be desired plans (e.g., following a straight path) or other high level intentional states (e.g., not following the rules of the road). Let the M -vector b i denote the intention vector of OV i ; the j th entry b j i corresponds to the belief OV i is operating under the jth intention. The cost functional (7) is then modified to embed the intention information, where T ij is the threat value of the trajectories followed by OV i as a realization of its j th intention; note that i ∈ {1 . . . N } and j ∈ {1 . . . M }.

B. Agent Model
Each agent is modeled using the standard bicycle model where (x, y) is the rear axle position, v is the forward speed, θ is the heading, L is the wheelbase, a is the forward acceleration, and δ is the steering angle (positive counterclockwise). The state of the vehicle is s = (x, y, θ, v) ∈ S, while the input is u = (δ, a) ∈ U, including the constraints a min ≤ a ≤ a max and |δ| ≤ δ max . Each admissible control sequence u : [0, t f ] → U is piecewise constant.
In the work that follows, we make several assumptions to constrain the focus on the uncertainty in the prediction of obstacles or other agents. We assume that the current states and models of the HV and the OVs are perfectly known by the HV within a detection radius (Section V-C), and that all sensor measurements are noise-free. Additionally, the path planner is provided with a complete map of the environment a priori, excluding any dynamic obstacles or agents.
IV. DEVELOPED ALGORITHMS This section describes the implementation of the IP and TA components of the TAM and the resulting threat-aware path planner, using closed-loop rapidly exploring random trees (CL-RRT), into which they are embedded.

A. Intention Predictor (IP)
The Ecological Recognizer architecture [10] that the IP adopts has two key components: a pattern matcher and a reasoning module. The pattern matcher is a classifier that is trained offline to recognise different possible intentions by observing agent states, and operates online by giving a continuous estimate of the intentions over time. The reasoning module filters these estimates of intentions, along with knowledge of previous encounters with the agent, to give a final estimate of the intention vector b.
Here we are interested in estimating the intentions of human vehicles in urban environments, specifically near intersections. This Ecological Recognizer implementation is based on an approach previously developed and demonstrated by the authors in simulation [11] for classifying agent intentions using support vector machines (SVM) and Bayesian filtering (BF). The human driver intention classification problem is very complex, due to the various nuances of human behaviors. SVM is suitable for the IP because it has been shown to be both robust and efficient for classification problems [12], such as lane-change detection [13].
The IP design consists of an SVM combined with a Bayesian filter that uses the SVM outputs over a specific time period to compute the intention vector b. SVM-BF also includes a threshold detector, such that the final vector b is a unit basis vector specifying the most likely intention according to the threshold value [11]. Based on experimenting with different combinations of kernel functions and features, the best results were obtained using the Gaussian radial basis function and combining the following three features: 1) the relative distance between the OV i and the entrance of the intersection it is approaching, 2) the speed of the OV i , and 3) the longitudinal acceleration of the OV i . Note that the SVM-BF algorithm is only activated when the distance between the OV i and the HV is within some detection radius, and both vehicles are approaching the same intersection.

B. Threat Assessor (TA)
The threat-aware CL-RRT algorithm (Section IV-C) incorporates the computation of a threat value for each candidate trajectory it generates, given knowledge of the current Algorithm 1 Intention-based Threat Assessment 1: T i ← 0 ∀i 2: for each agent OV i do 3: Reach-Set ← Compute-Intention-Reachability (b i ) 4: for each intention e j with probability b j i do 5: T i ← T i + b j i · Compute-Threat(Reach-Set,OV i , e j ) 6: end for 7: end for 8: return max i=1...N T i Algorithm 2 Compute-Threat (Reach-Set, OV, e) 1: for each path path k in Reach-Set ending in region of intention e do 2: t k ← compute earliest time of collision of path k with HV within time horizon T h 3: T k ← 1 t k 4: end for 5: return max k T k OV states. We propose an approach, detailed in Algorithm 1, that combines a fast sampling-based reachability method with intention prediction information provided by the IP (Section IV-A) to efficiently estimate the threat level.
The threat assessor has a finite time horizon, limiting the HV lookahead horizon for possible future OV paths, in order to focus computation on imminent threats. The choice of horizon length is domain-specific, but should allow the HV sufficient time to react in a dynamic environment. The Threat-Assessor algorithm (Algorithm 1) begins by calling the Compute-Intention-Reachability (CIR) subroutine, discussed below, to create the reachability set for each OV i . The resulting sets are biased based on the perceived intentions of the OVs. For each intention e j of each OV i , Algorithm 2 is called to compute the threat incurred by the HV trajectory in the region reached by the paths corresponding to e j . Note that the earliest time to collision is converted into a threat value using (6) (line 3 of Algorithm 2). This value is weighted by the probability b j i provided by the IP. Finally, the threat is computed as the maximum value of all threats created by each OV i (line 8 of Algorithm 1).
The CIR algorithm (detailed in [14]) is also based on the CL-RRT algorithm, but uses the resulting tree only for simulation of the different possible paths of a OV (which is not controlled by the operator). No best path is selected, but rather the entire tree is biased towards regions corresponding to the learned intentions of the OVs. This biasing is achieved by devoting a portion of the tree samples to regions corresponding to the intention e j , with remaining samples being taken uniformly throughout the environment. The algorithm also includes time-parametrization extensions (introduced in [15]) that tailor the RRT algorithm to the efficient computation of intented paths by the OVs.

C. Threat-Aware CL-RRT Planner
In this section, we present a global threat-aware planner which builds on the CL-RRT implementation developed for MIT's Talos vehicle in the 2007 DGC competition [16]. The CL-RRT algorithm extends the rapidly-exploring random tree (RRT) algorithm [4], [9], which grows a tree of dynamically feasible trajectories by randomly sampling points toward which the tree is extended. The CL-RRT algorithm adds a path-tracking control loop in the vehicle prediction model, such that sampling takes place in the reference input space rather than in the vehicle input space. The algorithm thus maintains the exploration bias of traditional RRT algorithms, while allowing for generation of smooth trajectories more efficiently. Furthermore, because the RRT algorithm is sampling-based, the quality of the planning tree and resulting paths are scalable with the available computational resources. Please refer to [16] for a detailed description of the CL-RRT planner, and [6] for an explanation of the planner integration in the Talos system architecture.
The proposed threat-aware planner is shown in Algorithm 3. The main addition is the threat computation in lines 6 and 10. This computation calls Algorithm 1 to efficiently calculate the threat of colliding with the possible paths of the other OVs (Section IV-B). Whereas the original CL-RRT heuristic identifies nearest nodes based on path duration (in the Dubins sense), we extend this heuristic to also include the threat value (line 6). In line 10, the threat along the edge to a new node is computed and stored at the new node. The best path is then chosen in accordance with the cost defined in (9) (line 15 of Algorithm 3), which includes both the total time to the goal and the threat along the trajectory. These modifications embed the threat computation in the CL-RRT trajectory generation, leading to the selection of safer paths which are threat-aware by construction.
Another related addition is the use of threat propagation logic. First, the root node is initialized to have zero threat. When adding a new node, if the path from the selected node (initially the root) to the new node incurs a non-zero threat (computed using Algorithm 1), then an intermediate node is created at the location of the earliest collision, and both the intermediate node and the new node inherit the computed threat value. Note that if a node with a non-zero threat value is chosen to be expanded, the new node inherits that value and no new threat computation is required. The rationale is that the threat value does not decrease along an edge connecting a node with non-zero threat to its child (recall (6)). A useful property of the threat heuristic is that it is admissible: it underestimates the "true" cost. This is due to the sampling-based nature of the CIR algorithm, which underapproximates the earliest time of collision, as well as the assumption that children nodes inherit the threat value of their parent if the value is non-zero. Before choosing a best path, the threat values of the tree are updated from the bottom up (i.e., starting from the leafs) by reassigning each parent's threat value to be the minimum of the threat of its children, and so on, until the root is reached.

V. EXPERIMENTAL RESULTS
This section presents experimental results which validate the effectiveness of the threat-aware CL-RRT algorithm, augmented with the TAM, in enabling an autonomous vehicle to successfully identify and avoid an errant human-driven vehicle under real-world uncertainty. These results focus on Algorithm 3 Threat-Aware CL-RRT 1: repeat 2: Measure current vehicle states and environment 3: Propagate states by computation time limit 4: repeat 5: Generate sample for the input to controller 6: Sort nodes in tree using threat and time heuristics 7: for each sorted node do 8: Form controller input by drawing line from node to sample, then propagate 9: if propagated portion is collision free then 10: Compute threat of propagated portion (Algorithm 2) 11: Add sample to tree break 12: end if 13: end for 14: until time limit is reached 15: Choose best path and repropagate from current states 16: if repropagated trajectory is infeasible then 17: Remove infeasible portion from tree and go to line 15 18: end if 19: Send best path to controller 20: until vehicle reaches target the challenging task of avoiding collisions at intersections, in particular with errant drivers who do not observe a stop sign and enter the intersection out of turn.
First, a brief overview of the experimental infrastructure is provided. Next, we present a subset of hardware results showing the interaction between an errant human driver and an autonomous vehicle at intersections; additional experiments can be viewed in the video attachment or at http://acl.mit.edu/IROS10TAM.mp4. Further experiments and infrastructure details are given in [14].

A. Hardware
Hardware demonstrations were performed in the Real-time indoor Autonomous Vehicle test ENvironment (RAVEN) [8], a testbed which uses motion-capture cameras to provide high-fidelity vehicle state data. We have constructed a representative road network within the RAVEN testbed, including many intersection types and road signs, to emulate a realistic driving environment ( Figure 2).
All vehicles in this experiment use the iRobot Create platform [17], a skid-steered vehicle, modified with a software wrapper to emulate traditional automotive steering (10). Each experiment consists of 1 or 2 autonomous vehicles and 1 human-driven vehicle. The autonomous vehicles are controlled through an off-board wrapper which receives waypoints from the threat-aware CL-RRT software (Section V-B) and steers using pure pursuit [18]. The human-driven vehicle is controlled through a wireless steering wheel [19], including acceleration and brake pedals. A human driver may steer their vehicle through simple visual inspection, or via an onboard dual-camera ( Figure 2) interface which emulates the first-person driving perspective (Figure 3). This interface includes a real-time "GPS" display, which guides the driver through an intended sequence of waypoints (Section V-B).

B. Software
The planning software consists of two primary modules, discussed below. The navigator module accesses an abstract  representation of the road network and selects sequences of waypoints, yielding the current goal location for the vehicle. The CL-RRT motion planning module then uses local knowledge of the environment (e.g., obstacles, terrain, and other agents), including the TAM, to plan dynamically feasible paths to this goal.
The navigator module leverages a sparse representation of the RAVEN road network (Figure 4), constructed using the Route Network Data File (RNDF) specification from the 2007 DGC [20]. As the RNDF represents the traversability graph, an A * implementation is used to select the shortestdistance waypoint path in the graph between the current and desired waypoints, respecting lane directionality constraints [6]. As the vehicle (whether human or autonomous) approaches each intermediate or desired waypoint, the navigator selects the next waypoint on the list as the new target. An arbitrary list of desired waypoints is provided for each vehicle; the ultimate objective is to observe the vehicles' interactions at intersections.
The threat-aware CL-RRT algorithm (Algorithm 3) is implemented as an offboard, multi-threaded Java application, designed to support arbitrary vehicle and environment models in both simulation and hardware. The CIR algorithm [14] is embedded in this offboard planner as a separate thread which runs asynchronously for each OV. Over fixed time intervals (2s), the algorithm grows a new reachability tree based on the current OV position in RAVEN. At the end of each time interval, the reachability tree is relayed to the host RRT thread, which then relays it to the TA algorithm (Section IV-B). Figure 4 shows the display representation generated by the application. Fig. 4. Display view of the RAVEN road network for the host vehicle (blue chevron, path history in blue). The host uses the CL-RRT tree (brown edges, green nodes) to identify the best known path (red edges, black nodes) to the current waypoint (purple). This path should avoid all obstacles (black boxes), the human-driven vehicle (green circled chevron) and, if active, their reachability tree (yellow edges, orange nodes). The other autonomous vehicle is represented as the car-sized rectangle at bottom-center.

C. Results
The results below demonstrate the threat-aware closedloop RRT algorithm in the context of stopping at intersections. In each experiment, one human-driven vehicle and one or two autonomous vehicles are navigating through the RAVEN road network simultaneously. The human driver may choose to not stop at an intersection and instead enter it out of turn, violating the rules of the road. The human driver's intent is classified as either good or errant, based on whether or not they correctly approach the intersections. The objective of the autonomous vehicle(s) is then to not only avoid all collisions, but also minimize the risk of collision with an errant human driver in intersections.
The human-driven vehicle is perceived to be a threat if three conditions are satisfied: (1) the IP module has classified the drivers' behavior as errant; (2) the errant driver is within the host vehicle's detection radius ( Figure 4); and (3) both vehicles are approaching the same intersection. In this case, the autonomous vehicle is to avoid that vehicle's reachability tree constructed by the CIR algorithm; otherwise the reachability tree is not an active constraint.
Over the course of this experiment set, 20 intersection encounters occurred: 10 with one autonomous vehicle in the network, and 10 with two autonomous vehicles in the world. Of these encounters, only one resulted in a collision; in this case, the human-driven vehicle aggressively approached the autonomous vehicle, which had already stopped to avoid it. Figure 5 demonstrates two particular scenarios in which the human-driven vehicle interacts with an autonomous vehicle at an intersection. In Scenario 1 (Figure 5(a)-(c)), both vehicles are attempting to enter the same lane, with the human-driven vehicle approaching from the left and making a right turn, and the autonomous vehicle approaching from the right and making a left turn. The autonomous vehicle arrives first and, observing (through the IP) that the human driver is decelerating, plans a path through the intersection ( Figure 5(a)). After entering the intersection, however, the IP module detects the human driver accelerating, as if it will enter the intersection out of turn. The IP module classifies this behavior as errant, and thus the TA module seeks to minimize the corresponding threat level T . Though the autonomous vehicle cannot avoid the reachability tree's outer reaches, it does avoid the inner nodes (where the time to collision is lower) by quickly coming to a stop ( Figure 5(b)).
Once the errant driver is no longer a threat, the autonomous agent finishes crossing the intersection ( Figure 5(c)). In Scenario 2 ( Figure 5(d)), the autonomous driver is making a left turn, while the human-driven vehicle is crossing the intersection (from left to right in the figure). Here, the human-driven vehicle has correctly come to a stop at the intersection, and the IP module has classified it as a good driver. Unlike Figure 5(b), the autonomous driver then proceeds through the intersection, perceiving that the human driver is likely to obey the rules.

VI. CONCLUSION
This paper has introduced a new approach for path planning in urban environments subject to uncertainty in the intentions of other vehicles. This work combines an intention predictor (IP) and a threat assessor (TA) to create a threat assessment module (TAM), embeddable in path planning algorithms, which runs efficiently and is suitable for realtime implementation. The IP uses high-level logic based on the Ecological Recogniser, while the TA uses sampling-based techniques to efficiently compute reachable future paths for surrounding vehicles. Using TAM applied to the CL-RRT planner, an autonomous vehicle can successfully identify and avoid an errant driver in realistic intersection scenarios, as demonstrated through hardware results.