Autonomous Cyber-Physical Systems: Path Planning Spring 2019. CS 599. Instructor: Jyo Deshmukh USC Viterbi School of Engineering Department of Computer Science Decision-making hierarchy Road topology, network Userspecified destination Route Planning Perception layer outputs Sequence of way-points Behavioral Planning Lane following

Vehicle state estimates Trajectory planning/Moti Reference Motion specification on planning path Adaptive cruising Collision avoidance Lane changing Intersection negotiation School of Engineering Department of Computer Science 2 USC Viterbi Estimated pose and free space Low-level Feedback

control Steering and throttle commands Route planning Represent road network/topology as a directed graph Nodes represent way-points Edges represent a cost of traveling between nodes Cost can represent time, distance, fuel economy, a weighted combination of some of these, etc. Simplest solutions based on Dijkstras shortest-path or A* search algorithms In practice, these do not scale well Lots of different algorithms proposed in the literature5 USC Viterbi School of Engineering Department of Computer Science 3 D Dijkstras Shortest path algorithm 5

6 ShortestPath (Graph G(V, E), src) 1 2 2 3 3 4 4 5 5 6 6 7 7 ;; USC Viterbi School of Engineering Department of Computer Science 10 10 10 10

9 9 9 9 4 C E E E E F F F F 3 3 3 3 3 3 3 3 3 3 3 3

B 6 5 3 B 1 5 A A F C D A A A A A A A A

A A A A 6 6 6 6 6 6 6 6 6 6 6 6 E 2 E A A A A A A A

A A A A A 6 6 5 5 5 5 5 5 5 5 5 5 F A A C C C C C C C

C C C 8 8 8 8 8 8 8 8 8 8 Q C C C C C C C C C C Dijkstras shortest path algorithm

Computes for each node in the graph, the shortest path from the source to that node Classic example of a dynamic programming algorithm Original algorithm runs in Optimization using Fibonnaci Heaps allows complexity Does not work for graphs with cycles or negative weights! Bellman-Ford algorithm for graphs with negative weights and cycles (but no negative cycles) Floyd-Warshall for finding shortest paths between all pairs of nodes USC Viterbi School of Engineering Department of Computer Science 5 Behavior planning Navigate selected route while interacting with traffic participants (pedestrians, bicycles, other cars). Behavioral layer responsible for selecting appropriate driving action based on perceived mode (as dictated by other participants, road conditions, etc.)

Finite State Machine-based approach: Model driving contexts and behaviors as finite sets, and use finite state machines to represent possible behaviors. Planning then becomes a problem of choosing appropriate transitions that ensure a desired behavior. Can view this is a 2-player game between the cars controller (player 1) and the environment (player 2). Game-theoretic approaches for FSMs can identify winning strategies for the controller. USC Viterbi School of Engineering Department of Computer Science 6 Behavioral planning under uncertainty FSM-based view may not adequately address uncertainty Uncertainty exists in the intention of traffic participants Intention prediction of cars, bicycles, pedestrians has been researched Techniques based on Gaussian mixture models Switching linear dynamical and stochastic systems Gaussian Process Regression

A big new push is probabilistic planning formalisms using Markov Decision Processes such as Partially Observable Markov Decision Processes We will learn about MDPs/PoMDPs in the lecture on reinforcement learning USC Viterbi School of Engineering Department of Computer Science 7 Motion planning Behavior plan decides a high-level set of maneuvers (e.g. cruise-in-lane, turn right, turn left, etc.) This plan needs to be translated into a reference path or a reference trajectory to be followed by the vehicle. Motion planning computes a feasible reference plan that respects feasibility (according to vehicle dynamics), obstacle avoidance, while optimizing certain costs (e.g. maximizing comfort, minimizing battery usage, etc.) Popular approaches Variational methods from nonlinear optimal control Graph-search approaches using discretization of the state-space Incremental tree-based methods USC Viterbi

School of Engineering Department of Computer Science 8 Path planning vs. Trajectory planning Path planning: Simply provides a parametric function is the configuration space or state-space of the vehicle gives the initial configuration, and gives the final configuration, and for all intermediate , gives the intermediate configurations May not include a velocity profile or those for other states, but delegates this to lower-level control Trajectory planning: Is aware of vehicle dynamics and can account for dynamic obstacles USC Viterbi School of Engineering Department of Computer Science 9 Trajectory planning Defines a parametric function , where

is the planning horizon, and is the set of configurations precisely describes how the vehicles behavior evolves in configuration space at each time Trajectory planning is a more nuanced version of path planning USC Viterbi School of Engineering Department of Computer Science 10 Trajectory planning setup : set of all continuous functions from to Let , then: (where is a set of initial configurations/states) (where is a set of goal states) Set of all allowed states at time denoted as encodes holonomic constraints A constraint is holonomic if you can express it as , where the s are coordinates, i.e. constraint depends only on spatial configuration Holonomic constraints can encode path constraints, and dynamic and static obstacles USC Viterbi School of Engineering Department of Computer Science

11 Trajectory planning setup continued is a differential constraint specifying constraints on smoothness of the path, velocity, acceleration, jerk constraints etc. is a cost functional mapping any trajectory to some cost in Optimal trajectory can be stated as follows: USC Viterbi School of Engineering Department of Computer Science 12 Trajectory planning setup continued Trajectory planning in dynamic environments and static path planning are both PSPACE-hard problems At least as complex as checking SAT of a quantified Boolean formula (QBF) Example QBF formula: USC Viterbi School of Engineering Department of Computer Science

13 Variational methods Rely on results from nonlinear continuous optimization We transform the trajectory optimization to the following standard form: Standard trick to go from constrained to unconstrained optimization is to introduce penalty functions Penalty method: Barrier function method: () USC Viterbi School of Engineering Department of Computer Science 14 Variational methods

Common to use logarithmic barrier functions with Newton-like iterative methods One problem: Trajectory space is infinite-dimensional. Solution: Direct methods: Restrict to finite-dimensional subspace, , where are some basis functions (e.g. piecewise linear interpolation uses PWL basis functions) Requires numerical integration approximations for the trajectory combined with optimizers like conjugate gradient descent Indirect methods: use Pontryagins minimum principle that gives optimality conditions for trajectory optimization problem Requires solving boundary-value problems of ODEs, that can be approximated numerically USC Viterbi School of Engineering Department of Computer Science 15 Graph-search-methods

Variational methods may get stuck in local optima, graph-search methods try to do global search Configuration space is represented as a graph , where is a set of selected discrete configurations, and is a set of edges, where each edge is labeled by the path segment that the edge represents. If , then and Feasible path can be found by concatenating feasible edges Does require discretization of the configuration/state space! Hand-crafted lane graphs2 Geometric representations Sampling-based discretization USC Viterbi School of Engineering Department of Computer Science 16 Some geometric representations3 Voronoi diagrams Occupancy Grid Map Cost Map State Lattice Driving Corridors

Several nice results or path planning using geometric representations, but usually computationally expensive! USC Viterbi School of Engineering Department of Computer Science 17 Sampling-based methods Typical constraints (differential, holonomic) are usually very difficult to obtain at real-time through sensors Sampling-based methods explore reachability of free configuration space by trying to apply controls (steering) and checking for collisions Main purpose of these methods is to construct a roadmap, and use it for planning Steering functions: Random : returns a path obtained by applying a random steering command Heuristic: tries to guide vehicle from point to point Exact: computes exact path that takes vehicle from to Optimal: Returns an optimal exact path

USC Viterbi School of Engineering Department of Computer Science 18 Probabilistic Roadmap (PRM) Construction for for Sample: Return points on a grid Random samples if endif end End return USC Viterbi School of Engineering Department of Computer Science Interesting: -nearest neighbors using appropriate

distance function Points within a ball of radius around 19 Trajectory plan: Use Dijkstras shortestpath algorithm on the resulting roadmap Heuristic search A* Once we have a roadmap, how do we find the best path in the roadmap? Dijkstras shortest path algorithm: uses best first search to build a tree representing shortest paths from a given source to all other vertices in the graph We typically need a path to only a single vertex (in the goal set), in this case a heuristic can be used to guide the search process Most prominent heuristic search algorithm is A* Shown to be optimally efficient if heuristic function is admissible (never overestimates the cost) USC Viterbi School of Engineering

Department of Computer Science 20 How A* works It constructs a tree of paths starting from the initial node, expanding paths one step at a time, till one of the path reaches the goal In each iteration, A* determines which of its partial paths to expand based on an estimate of the cost. It selects the path that minimizes , where, . Here, = cost of path from start node to , and is an estimate of the cheapest cost to go from to the goal Heuristic function is problem-specific. For the heuristic to be admissible, it should never over-estimate actual cost to get to the goal Dijkstras shortest path is a special case of A*, where USC Viterbi School of Engineering Department of Computer Science 21 A* implementation ;

; A* (Graph G(V, E), ) // newly discovered nodes, not yet explored // already evaluated nodes // best predecessor node so far // cost of reaching from // estimate of reaching from via USC Viterbi School of Engineering Department of Computer Science 22 Weighted A* Admissibility criterion forces A* to inspect all equally promising paths to find the optimal Approximate shortest paths can be found by relaxing the admissibility criterion (i.e. the function) This guarantees that the path found is no worse than times the optimal path Weighted A* variants involve defining a heuristic that is a function of . E.g.

static weighted A* uses USC Viterbi School of Engineering Department of Computer Science 23 Many variants of A* Real-time re-planning: D*, Focused D*, D* Lite: recompute shortest path anytime the underlying graph changes, making use of results from previous search Anytime search algorithms: Quickly find a (possibly sub-optimal) path first, and continue to improve the solution if there is more computational time Anytime A*: weighted heuristic that continues searching using cost of previously identified path as upper bound and admissible heuristic as lower bound Other variants such as Anytime Repairing A*, Anytime Dynamic A* etc. USC Viterbi School of Engineering Department of Computer Science 24 Rapidly Exploring Random Trees (RRTs)

RRT one of a family of algorithms based on incremental sampling, i.e. when you do not want to a priori build the entire roadmap Also known as single-query PRMs Very fast algorithm, allows on-the-fly construction of paths // is a tree for // Find point in that is closest to // Find control that gets you closest to from end Plan = Path from to s.t. USC Viterbi School of Engineering Department of Computer Science 25

RRT in action Obstacle Obstacle Obstacle Target USC Viterbi School of Engineering Department of Computer Science Target Target 26 Sample goal Select closest neighbor Find collision-free control Problems with RRT

Suppose you are given a function that assigns costs to paths RRT solution is usually far from optimal Karaman & Frazzoli (2010) showed that probability of RRT converging to an optimal solution is 0 First alternative: RRG (Rapidly exploring Random Graph) Connects to the new sample all vertices within some distance to it Can create graphs with cycles Improves optimality: probability of RRG converging to an optimal solution is 1 USC Viterbi School of Engineering Department of Computer Science 27 Many variants of PRMs and RRTs There are many variants that exist: RRT* (rewires the tree as better paths are found), PRM*, etc. Some of them add optimality criteria such as costs on edges

Algorithms differ in the underlying structure maintained: PRM (forest), RRT (tree), RRG (graphs with possible cycles), RRT* (acyclic graphs) etc. There are two key considerations: Probabilistic completeness: given infinite time, will the procedure identify a path to the goal set Asymptotic optimality: does the optimal path found by the algorithm converge to the actual optimum in the limit USC Viterbi School of Engineering Department of Computer Science 28 Combining RRTs with STL Define path planning objectives in STL Define robustness of STL on partial trajectories Use RRT to explore the search space When selecting nearest neighbors of the goal point, select ones with high robustness Allows path-planning with STL objectives! Combining RRTs with LTL objectives has been extensively studied in the literature

USC Viterbi School of Engineering Department of Computer Science 29 Bibliography 1. Pendleton, Scott Drew, Hans Andersen, Xinxin Du, Xiaotong Shen, Malika Meghjani, You Hong Eng, Daniela Rus, and Marcelo H. Ang. "Perception, planning, control, and coordination for autonomous vehicles." Machines 5, no. 1 (2017): 6. 2. Most material in this lecture is from this paper: Paden, Brian, et al. "A survey of motion planning and control techniques for self-driving urban vehicles." IEEE Transactions on Intelligent Vehicles 1.1 (2016): 3355. https://arxiv.org/pdf/1604.07446.pdf 3. Katrakazas, C., Quddus, M., Chen, W. H., & Deka, L. (2015). Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions. Transportation Research Part C: Emerging Technologies, 60, 416-442. 4. https://en.wikipedia.org/wiki/A*_search_algorithm 5.

Bast, D. Delling, A. Goldberg, M. Mller-Hannemann, T. Pajor, P. Sanders, D. Wagner, and R. F. Werneck, Route planning in transportation networks, https://arxiv.org/pdf/1504.05140.pdf USC Viterbi School of Engineering Department of Computer Science 30