Apr 25, 2016 - explicitly using a partially-observable Markov decision pro- cess (POMDP) framework and propose specific approximate solution strategie...

118 downloads 79 Views 6MB Size

No documents

A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles

arXiv:1604.07446v1 [cs.RO] 25 Apr 2016

ˇ ∗,1,2 , Sze Zheng Yong1 , Dmitry Yershov1 , and Emilio Frazzoli1 Brian Paden∗,1 , Michal Cáp

Abstract Self-driving vehicles are a maturing technology with the potential to reshape mobility by enhancing the safety, accessibility, efficiency, and convenience of automotive transportation. Safety-critical tasks that must be executed by a self-driving vehicle include planning of motions through a dynamic environment shared with other vehicles and pedestrians, and their robust executions via feedback control. The objective of this paper is to survey the current state of the art on planning and control algorithms with particular regard to the urban setting. A selection of proposed techniques is reviewed along with a discussion of their effectiveness. The surveyed approaches differ in the vehicle mobility model used, in assumptions on the structure of the environment, and in computational requirements. The side-by-side comparison presented in this survey helps to gain insight into the strengths and limitations of the reviewed approaches and assists with system level design choices.

C ONTENTS I

Introduction

II

Overview of the Decision-Making Hierarchy used in II-A Route Planning . . . . . . . . . . . . . . . . . II-B Behavioral Decision Making . . . . . . . . . . II-C Motion Planning . . . . . . . . . . . . . . . . II-D Vehicle Control . . . . . . . . . . . . . . . . .

2 Driverless . . . . . . . . . . . . . . . . . . . . . . . .

Cars . . . . . . . . . . . .

. . . .

3 3 3 4 4

III

Modeling for Planning and Control III-A The Kinematic Single-Track Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . III-B Inertial Effects . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4 4 6

IV

Motion IV-A IV-B IV-C IV-D

VI

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

7 7 9 9 11 11 12 12 14 15 16

Vehicle Control V-A Path Stabilization for the Kinematic Model . . . . . . . V-A1 Pure Pursuit . . . . . . . . . . . . . . . . . . V-A2 Rear wheel position based feedback . . . . . V-A3 Front wheel position based feedback . . . . V-B Trajectory Tracking Control for the Kinematic Model . V-B1 Control Lyapunov based design . . . . . . . V-B2 Output feedback linearization . . . . . . . . V-C Predictive Control Approaches . . . . . . . . . . . . . . V-C1 Unconstrained MPC with Kinematic Models V-C2 Path Tracking Controllers . . . . . . . . . . V-C3 Trajectory Tracking Controllers . . . . . . . V-D Linear Parameter Varying Controllers . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

17 18 18 19 19 20 20 21 21 22 22 22 23

IV-E IV-F V

Planning Path Planning . . . . . . . . . . . . Trajectory Planning . . . . . . . . Variational Methods . . . . . . . . Graph Search Methods . . . . . . . IV-D1 Lane Graph . . . . . . . IV-D2 Geometric Methods . . . IV-D3 Sampling-based Methods IV-D4 Graph Search Strategies Incremental Search Techniques . . Practical Deployments . . . . . . .

. . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

Conclusions

References

. . . . . . . . . .

23 23

∗

The first two authors contributed equally to this work. The authors are with the Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge MA, USA. email: 1

[email protected], [email protected], [email protected], [email protected], [email protected]

2 Michal Cáp ˇ is also affiliated with Dept. of Computer Science, Faculty of Electrical Engineering, CTU in Prague, Czech Republic.

2

I. I NTRODUCTION The last three decades have seen steadily increasing research efforts, both in academia and in industry, towards developing driverless vehicle technology. These developments have been fueled by recent advances in sensing and computing technology together with the potential transformative impact on automotive transportation and the perceived societal benefit: In 2014 there were 32,675 traffic related fatalities, 2.3 million injuries, and 6.1 million reported collisions [1]. Of these, an estimated 94% are attributed to driver error with 31% involving legally intoxicated drivers, and 10% from distracted drivers [2]. Autonomous vehicles have the potential to dramatically reduce the contribution of driver error and negligence as the cause of vehicle collisions. They will also provide a means of personal mobility to people who are unable to drive due to physical or visual disability. Finally, for the 86% of the US work force that commutes by car, on average 25 minutes (one way) each day [3], autonomous vehicles would facilitate more productive use of the transit time, or simply reduce the measurable ill effects of driving stress [4]. Considering the potential impacts of this new technology, it is not surprising that self-driving cars have had a long history. The idea has been around as early as in the 1920s, but it was not until the 1980s that driverless cars seemed like a real possibility. Pioneering work led by Ernst Dickmanns (e.g., [5]) in the 1980s paved the way for the development of autonomous vehicles. At that time a massive research effort, the PROMETHEUS project, was funded to develop an autonomous vehicle. A notable demonstration in 1994 resulting from the work was a 1,600 km drive by the VaMP driverless car, of which 95% was driven autonomously [6]. At a similar time, the CMU NAVLAB was making advances in the area and in 1995 demonstrated further progress with a 5,000 km drive across the US of which 98% was driven autonomously [7]. The next major milestone in driverless vehicle technology was the first DARPA Grand Challenge in 2004. The objective was for a driverless car to navigate a 150-mile off-road course as quickly as possible. This was a major challenge in comparison to previous demonstrations in that there was to be no human intervention during the race. Although prior works demonstrated nearly autonomous driving, eliminating human intervention at critical moments proved to be a major challenge. None of the 15 vehicles entered into the event completed the race. In 2005 a similar event was held; this time 5 of 23 teams reached the finish line [8]. Later, in 2007, the DARPA Urban Challenge was held, in which vehicles were required to drive autonomously in a simulated urban setting. Six teams finished the event demonstrating that fully autonomous urban driving is possible [9]. Numerous events and major autonomous vehicle system tests have been carried out since the DARPA challenges. Notable examples include the Intelligent Vehicle Future Challenges from 2009 to 2013 [10], Hyundai Autonomous Challenge in 2010 [11], the VisLab Intercontinental Autonomous Challenge in 2010 [12], the Public Road Urban DriverlessCar Test in 2013 [13], and the autonomous drive of the

Bertha-Benz historic route [14]. Simultaneously, research has continued at an accelerated pace in both the academic setting as well as in industry. The Google self-driving car [15] and Tesla’s Autopilot system [16] are two examples of commercial efforts receiving considerable media attention. The extent to which a car is automated can vary from fully human operated to fully autonomous. The SAE J3016 standard [17] introduces a scale from 0 to 5 for grading vehicle automation. In this standard, the level 0 represents a vehicle where all driving tasks are the responsibility of a human driver. Level 1 includes basic driving assistance such as adaptive cruise control, anti-lock braking systems and electronic stability control [18]. Level 2 includes advanced assistance such as hazard-minimizing longitudinal/lateral control [19] or emergency braking [20], [21], often based upon set-based formal control theoretic methods to compute ‘worst-case’ sets of provably collision free (safe) states [22]–[24]. At level 3 the system monitors the environment and can drive with full autonomy under certain conditions, but the human operator is still required to take control if the driving task leaves the autonomous system’s operational envelope. A vehicle with level 4 automation is capable of fully autonomous driving in certain conditions and will safely control the vehicle if the operator fails to take control upon request to intervene. Level 5 systems are fully autonomous in all driving modes. The availability of on-board computation and wireless communication technology allows cars to exchange information with other cars and with the road infrastructure giving rise to a closely related area of research on connected intelligent vehicles [25]. This research aims to improve the safety and performance of road transport through information sharing and coordination between individual vehicles. For instance, connected vehicle technology has a potential to improve throughput at intersections [26] or prevent formation of traffic shock waves [27]. To limit the scope of this survey, we focus on aspects of decision making, motion planning, and control for self-driving cars, in particular, for systems falling into the automation level of 3 and above. For the same reason, the broad field of perception for autonomous driving is omitted and instead the reader is referred to a number of comprehensive surveys and major recent contributions on the subject [28]–[31]. The decision making in contemporary autonomous driving systems is typically hierarchically structured into route planning, behavioral decision making, local motion planning and feedback control. The partitioning of these levels are, however, rather blurred with different variations of this scheme occurring in the literature. This paper provides a survey of proposed methods to address these core problems of autonomous driving. Particular emphasis is placed on methods for local motion planning and control. The remainder of the paper is structured as follows: In Section II, a high level overview of the hierarchy of decision making processes and some of the methods for their design are presented. Section III reviews models used to approximate the mobility of cars in urban settings for the purposes of motion planning and feedback control. Section IV surveys the rich literature on motion planning and discusses its applicability for

3

self-driving cars. Similarly, Section V discusses the problems of path and trajectory stabilization and specific feedback control methods for driverless cars. Lastly, Section VI concludes with remarks on the state of the art and potential areas for future research. II. OVERVIEW OF THE D ECISION -M AKING H IERARCHY USED IN D RIVERLESS C ARS In this section we describe the decision making architecture of a typical self-driving car and comment on the responsibilities of each component. Driverless cars are essentially autonomous decision-making systems that process a stream of observations from on-board sensors such as radars, LIDARs, cameras, GPS/INS units, and odometry. These observations, together with prior knowledge about the road network, rules of the road, vehicle dynamics, and sensor models, are used to automatically select values for controlled variables governing the vehicle’s motion. Intelligent vehicle research aims at automating as much of the driving task as possible. The commonly adopted approach to this problem is to partition and organize perception and decision-making tasks into a hierarchical structure. The prior information and collected observation data are used by the perception system to provide an estimate of the state of the vehicle and its surrounding environment; the estimates are then used by the decisionmaking system to control the vehicle so that the driving objectives are accomplished. The decision making system of a typical self-driving car is hierarchically decomposed into four components (cf. Figure II.1): At the highest level a route is planned through the road network. This is followed by a behavioral layer, which decides on a local driving task that progresses the car towards the destination and abides by rules of the road. A motion planning module then selects a continuous path through the environment to accomplish a local navigational task. A control system then reactively corrects errors in the execution of the planned motion. In the remainder of the section we discuss the responsibilities of each of these components in more detail. A. Route Planning At the highest level, a vehicle’s decision-making system must select a route through the road network from its current position to the requested destination. By representing the road network as a directed graph with edge weights corresponding to the cost of traversing a road segment, such a route can be formulated as the problem of finding a minimum-cost path on a road network graph. The graphs representing road networks can however contain millions of edges making classical shortest path algorithms such as Dijkstra [32] or A* [33] impractical. The problem of efficient route planning in transportation networks has attracted significant interest in the transportation science community leading to the invention of a family of algorithms that after a one-time pre-processing step return an optimal route on a continent-scale network in milliseconds [34], [35]. For a comprehensive survey and comparison of practical algorithms that can be used to efficiently plan routes for both human-driven and self-driving vehicles, see [36].

User speciﬁied destination

Route Planning Road network data

Sequence of waypoints through road network Perceived agents, obstacles, and signage

Behavioral Layer Negotiate Intersection

Parking Maneuver

Lane Following

Change Lanes

Unstructured Environment

Motion Speciﬁcation Estimated pose and collision free space

Motion Planning

Reference path or trajectory

Estimate of vehicle state

Local Feedback Control

Steering, throttle and brake commands

Figure II.1: Illustration of the hierarchy of decision-making processes. A destination is passed to a route planner that generates a route through the road network. A behavioral layer reasons about the environment and generates a motion specification to progress along the selected route. A motion planner then solves for a feasible motion accomplishing the specification. A feedback control adjusts actuation variables to correct errors in executing the reference path.

B. Behavioral Decision Making After a route plan has been found, the autonomous vehicle must be able to navigate the selected route and interact with other traffic participants according to driving conventions and rules of the road. Given a sequence of road segments specifying the selected route, the behavioral layer is responsible for selecting an appropriate driving behavior at any point of time based on the perceived behavior of other traffic participants, road conditions, and signals from infrastructure. For example, when the vehicle is reaching the stop line before an intersection, the behavioral layer will command the vehicle

4

to come to a stop, observe the behavior of other vehicles, bikes, and pedestrians at the intersection, and let the vehicle proceed once it is its turn to go. Driving manuals dictate qualitative actions for specific driving contexts. Since both driving contexts and the behaviors available in each context can be modeled as finite sets, a natural approach to automating this decision making is to model each behavior as a state in a finite state machine with transitions governed by the perceived driving context such as relative position with respect to the planned route and nearby vehicles. In fact, finite state machines coupled with different heuristics specific to considered driving scenarios were adopted as a mechanism for behavior control by most teams in the DARPA Urban Challenge [9]. Real-world driving, especially in an urban setting, is however characterized by uncertainty over the intentions of other traffic participants. The problem of intention prediction and estimation of future trajectories of other vehicles, bikes and pedestrians has also been studied. Among the proposed solution techniques are machine learning based techniques, e.g., Gaussian mixture models [37], Gaussian process regression [38], the learning techniques reportedly used in Google’s self-driving system for intention prediction [39], and modelbased approaches for directly estimating intentions from sensor measurements [40], [41]. This uncertainty in the behavior of other traffic participants is commonly considered in the behavioral layer for decision making using probabilistic planning formalisms, such as Markov Decision Processes (MDPs) and their generalizations. For example, [42] formulates the behavioral decision-making problem in the MDP framework. Several works [43]–[46] model unobserved driving scenarios and pedestrian intentions explicitly using a partially-observable Markov decision process (POMDP) framework and propose specific approximate solution strategies. C. Motion Planning When the behavioral layer decides on the driving behavior to be performed in the current context, which could be, e.g., cruise-in-lane, change-lane, or turn-right, the selected behavior has to be translated into a path or trajectory that can be tracked by the low-level feedback controller. The resulting path or trajectory must be dynamically feasible for the vehicle, comfortable for the passenger, and avoid collisions with obstacles detected by the on-board sensors. The task of finding such a path or trajectory is a responsibility of the motion planning system. The task of motion planning for an autonomous vehicle corresponds to solving the standard motion planning problem as discussed in the robotics literature. Exact solutions to the motion planning problem are in most cases computationally intractable. Thus, numerical approximation methods are typically used in practice. Among the most popular numerical approaches are variational methods that pose the problem as non-linear optimization in a function space, graph-search approaches that construct graphical discretization of the vehicle’s state space and search for a shortest path using graph search

methods, and incremental tree-based approaches that incrementally construct a tree of reachable states from the initial state of the vehicle and then select the best branch of such a tree. The motion planning methods relevant for autonomous driving are discussed in greater detail in Section IV. D. Vehicle Control In order to execute the reference path or trajectory from the motion planning system a feedback controller is used to select appropriate actuator inputs to carry out the planned motion and correct tracking errors. The tracking errors generated during the execution of a planned motion are due in part to the inaccuracies of the vehicle model. Thus, a great deal of emphasis is placed on the robustness and stability of the closed loop system. Many effective feedback controllers have been proposed for executing the reference motions provided by the motion planning system. A survey of related techniques are discussed in detail in Section V. III. M ODELING FOR P LANNING AND C ONTROL In this section we will survey the most commonly used models of mobility of car-like vehicles. Such models are widely used in control and motion planning algorithms to approximate a vehicle’s behavior in response to control actions in relevant operating conditions. A high-fidelity model may accurately reflect the response of the vehicle, but the added detail may complicate the planning and control problems. This presents a trade-off between the accuracy of the selected model and the difficulty of the decision problems. This section provides an overview of general modeling concepts and a survey of models used for motion planning and control. Modeling begins with the notion of the vehicle configuration, representing its pose or position in the world. For example, configuration can be expressed as the planar coordinate of a point on the car together with the car’s heading. This is a coordinate system for the configuration space of the car. This coordinate system describes planar rigid-body motions (represented by the Special Euclidean group in two dimensions, SE(2)) and is a commonly used configuration space [47]–[49]. Vehicle motion must then be planned and regulated to accomplish driving tasks and while respecting the constraints introduced by the selected model. A. The Kinematic Single-Track Model In the most basic model of practical use, the car consists of two wheels connected by a rigid link and is restricted to move in a plane [48]–[52]. It is assumed that the wheels do not slip at their contact point with the ground, but can rotate freely about their axes of rotation. The front wheel has an added degree of freedom where it is allowed to rotate about an axis normal to the plane of motion. This is to model steering. These two modeling features reflect the experience most passengers have where the car is unable to make lateral displacement without simultaneously moving forward. More formally, the limitation on maneuverability is referred to as a nonholonomic constraint [47], [53]. The nonholonomic constraint is

5

expressed as a differential constraint on the motion of the car. This expression varies depending on the choice of coordinate system. Variations of this model have been referred to as the car-like robot, bicycle model, kinematic model, or single track model. The following is a derivation of the differential constraint in several popular coordinate systems for the configuration. In reference to Figure III.1, the vectors pr and pf denote the location of the rear and front wheels in a stationary or inertial coordinate system with basis vectors (ˆ ex , eˆy , eˆz ). The heading θ is an angle describing the direction that the vehicle is facing. This is defined as the angle between vectors eˆx and pf − pr . Differential constraints will be derived for the coordinate systems consisting of the angle θ, together with the motion of one of the points pr as in [54], and pf as in [55].

Figure III.1: Kinematics of the single track model. pr and pf are the ground contact points of the rear and front tire respectively. θ is the vehicle heading. Time derivatives of pr and pf are restricted by the nonholonomic constraint to the direction indicated by the blue arrows. δ is the steering angle of the front wheel. The motion of the points pr and pf must be collinear with the wheel orientation to satisfy the no-slip assumption. Expressed as an equation, this constraint on the rear wheel is (p˙r · eˆy ) cos(θ) − (p˙r · eˆx ) sin(θ) = 0,

(III.1)

and for the front wheel: (p˙f · eˆy ) cos(θ + δ) − (p˙f · eˆx ) sin(θ + δ) = 0.

(III.2)

This expression is usually rewritten in terms of the componentwise motion of each point along the basis vectors. The motion of the rear wheel along the eˆx -direction is xr := pr · eˆx . Similarly, for eˆy -direction, yr := pr · eˆy . The forward speed is vr := p˙r · (pf − pr )/k(pf − pr )k, which is the magnitude of p˙r with the correct sign to indicate forward or reverse driving. In terms of the scalar quantities xr , yr , and θ, the differential constraint is x˙ r = vr cos(θ), y˙ r = vr sin(θ), (III.3) θ˙ = vlr tan(δ). Alternatively, the differential constraint can be written in terms

the motion of pf , x˙ f y˙ f θ˙

= = =

vf cos(θ + δ), vf sin(θ + δ), vf l sin(δ),

(III.4)

where the front wheel forward speed vf is now used. The front wheel speed, vf , is related to the rear wheel speed by vr = cos(δ). (III.5) vf The planning and control problems for this model involve selecting the steering angle δ within the mechanical limits of the vehicle δ ∈ [δmin , δmax ], and forward speed vr within an acceptable range, vr ∈ [vmin , vmax ]. A simplification that is sometimes utilized, e.g. [56], is to select the heading rate ω instead of steering angle δ. These quantities are related by lω , (III.6) δ = arctan vr simplifying the heading dynamics to i hv vr r tan (δmin ) , tan (δmax ) . (III.7) θ˙ = ω, ω ∈ l l In this situation, the model is sometimes referred to as the unicycle model since it can be derived by considering the motion of a single wheel. An important variation of this model is the case when vr is fixed. This is sometimes referred to as the Dubins car, after Lester Dubins who derived the minimum time motion between to points with prescribed tangents [57]. Another notable variation is the Reeds-Shepp car for which minimum length paths are known when vr takes a single forward and reverse speed [58]. These two models have proven to be of some importance to motion planning and will be discussed further in Section IV. The kinematic models are suitable for planning paths at low speeds (e.g. parking maneuvers and urban driving) where inertial effects are small in comparison to the limitations on mobility imposed by the no-slip assumption. A major drawback of this model is that it permits instantaneous steering angle changes which can be problematic if the motion planning module generates solutions with such instantaneous changes. Continuity of the steering angle can be imposed by augmenting (III.4), where the steering angle integrates a commanded rate as in [49]. Equation (III.4) becomes x˙ f y˙ f θ˙ δ˙

= = = =

vf cos(θ + δ), vf sin(θ + δ), vf l sin(δ), vδ .

(III.8)

In addition to the limit on the the steering rate h steering angle, i ˙ ˙ can now be limited: vδ ∈ δmin , δmax . The same problem can arise with the car’s speed vr and can be resolved in the same way. The drawback to this technique is the increased dimension of the model which can complicate motion planning and control problems. The choice of coordinate system is not limited to using one of the wheel locations as a position coordinate. For models

6

derived using principles from classical mechanics it can be convenient to use the center of mass as the position coordinate as in [59], [60], or the center of oscillation as in [61], [62]. B. Inertial Effects When the acceleration of the vehicle is sufficiently large, the no-slip assumption between the tire and ground becomes invalid. In this case a more accurate model for the vehicle is as a rigid body satisfying basic momentum principles. That is, the acceleration is proportional to the force generated by the ground on the tires. Taking pc to be the vehicles center of mass, and a coordinate of the configuration (cf. Figure III.2), the motion of the vehicle is governed by m¨ pc = Ff + Fr , Izz θ¨ = (pc − pf ) × Ff + (pc − pr ) × Fr ,

(III.9)

where Fr and Ff are the forces applied to the vehicle by the ground through the ground-tire interaction, m is the vehicles total mass, and Izz is the polar moment of inertia in the eˆz direction about the center of mass. In the following derivations we tacitly neglect the motion of pc in the eˆz direction with the assumptions that the road is level, the suspension is rigid and vehicle remains on the road. The expressions for Fr and Ff vary depending on modeling assumptions [18], [59], [60], [62], but in any case the expression can be tedious to derive. Equations (III.10)-(III.15) therefore provide a detailed derivation as a reference. The force between the ground and tires is modeled as being dependent on the rate that the tire slips on the ground. Although the center of mass serves as a coordinate for the configuration, the velocity of each wheel relative to the ground is needed to determine this relative speed. The kinematic relations between these three points are −lr cos(θ) pr = pc + −lr sin(θ) , 0 0 −lr cos(θ) p˙r = p˙c + 0 × −lr sin(θ) , ˙ θ 0 (III.10) lf cos(θ) pf = pc + lf sin(θ) , 0 0 lf cos(θ) p˙f = p˙c + 0 × lf sin(θ) . 0 θ˙

T and R = 0, 0, −r . The wheel radius is the scalar quantity r, and Ω{r,f } are the angular speeds of each wheel relative to the car. This is illustrated for the rear wheel in Figure III.3. Under static conditions, or when the height of the center of mass can be approximated as pc · eˆz ≈ 0, the component of the force normal to the ground, F{r,f } · eˆz can be computed from a static force-torque balance as Ff · eˆz =

lr mg , lf + lr

Fr · eˆz =

lf mg . lf + lr

(III.13)

The normal force is then used to compute the traction force on each tire together with the slip and a friction coefficient model, µ, for the tire behavior. The traction force on the rear tire is given component-wise by ks k

Fr · eˆx

=

Fr · eˆy

=

(Fr ·ˆ ez )µ( Ωrrr )sr ksr k ks k (Fr ·ˆ ez )µ( Ωrrr )sr − ksr k

−

· eˆx ,

(III.14)

· eˆy .

The same expression describes the front tire with the rsubscript replaced by an f -subscript. The formula above models the traction force as being anti-parallel to the slip with magnitude proportional to the normal force with a nonlinear

Figure III.2: Illustration of single track model kinematics without the no-slip assumption. ω{r,f } are relative angular velocities of the wheels with respect to the vehicle.

These kinematic relations are used to determine the velocities of the point on each tire in contact with the ground, sr and sf . The velocity of these points are referred to as the tire slip velocity. In general, sr and sf differ from p˙r and p˙f through the angular velocity of the wheel. The kinematic relation is sr sf

= =

p˙r + ωr × R, p˙f + ωf × R.

(III.11)

The angular velocities of the wheels are given by Ωr sin(θ) Ωf sin(θ + δ) ωr = −Ωr cos(θ) , ωf = −Ωf cos(θ + δ) , 0 0 (III.12)

Figure III.3: Illustration of the rear wheel kinematics in two dimensions showing the wheel slip, sr , in relation to the rear wheel velocity, p˙r , and angular speed, Ωr . In general, sr and p˙r are not collinear and may have nonzero components normal to the plane depicted.

7

dependence on the slip ratio (the magnitude of the slip normalized by Ωr r for the rear and Ωf r for the front). Combining (III.10)-(III.15) yields expressions for the net force on each wheel of the car in terms of the control variables, generalized coordinates, and their velocities. Equation (III.14), together with the following model for µ, ksr k ksr k = D sin C arctan B , (III.15) µ Ωr r Ωr r are a frequently used model for tire interaction with the ground. Equation (III.15) is a simplified version of the well known model due to Pacejka [63]. The rotational symmetry of (III.14) together with the peak in (III.15) lead to a maximum norm force that the tire can exert in any direction. This peak is referred to as the friction circle depicted in Figure III.4.

of travel, the center of the stop line at the next intersection, or the next desired parking spot. The motion planning component accepts information about static and dynamic obstacles around the vehicle and generates a collision-free trajectory that satisfies dynamic and kinematic constraints on the motion of the vehicle. Oftentimes, the motion planner also minimizes a given objective function. In addition to travel time, the objective function may penalize hazardous motions or motions that cause passenger discomfort. In a typical setup, the output of the motion planner is then passed to the local feedback control layer. In turn, feedback controllers generate an input signal to regulate the vehicle to follow this given motion plan. A motion plan for the vehicle can take the form of a path or a trajectory. Within the path planning framework, the solution path is represented as a function σ(α) : [0, 1] → X , where X is the configuration space of the vehicle. Note that such a solution does not prescribe how this path should be followed and one can either choose a velocity profile for the path or delegate this task to lower layers of the decision hierarchy. Within the trajectory planning framework, the control execution time is explicitly considered. This consideration allows for direct modeling of vehicle dynamics and dynamic obstacles. In this case, the solution trajectory is represented as a timeparametrized function π(t) : [0, T ] → X , where T is the planning horizon. Unlike a path, the trajectory prescribes how the configuration of the vehicle evolves over time. In the following two sections, we provide a formal problem definition of the path planning and trajectory planning problems and review the main complexity and algorithmic results for both formulations. A. Path Planning

Figure III.4: A zoomed in view of the wheel slip to traction force map at each tire (top) and a zoomed out view emphasizing the peak that defines the friction circle (bottom); cf. Equation (III.15). The models discussed in this section appear frequently in the literature on motion planning and control for driverless cars. They are suitable for the motion planning and control tasks discussed in this survey. However, lower level control tasks such as electronic stability control and active suspension systems typically use more sophisticated models for the chassis, steering and, drive-train. IV. M OTION P LANNING The motion planning layer is responsible for computing a safe, comfortable, and dynamically feasible trajectory from the vehicle’s current configuration to the goal configuration provided by the behavioral layer of the decision making hierarchy. Depending on context, the goal configuration may differ. For example, the goal location may be the center point of the current lane a number of meters ahead in the direction

The path planning problem is to find a path σ(α) : [0, 1] → X in the configuration space X of the vehicle (or more generally, a robot) that starts at the initial configuration and reaches the goal region while satisfying given global and local constraints. Depending on whether the quality of the solution path is considered, the terms feasible and optimal are used to describe this path. Feasible path planning refers to the problem of determining a path that satisfies some given problem constraints without focusing on the quality of the solution; whereas optimal path planning refers to the problem of finding a path that optimizes some quality criterion subject to given constraints. The optimal path planning problem can be formally stated as follows. Let X be the configuration space of the vehicle and let Σ(X ) denote the set of all continuous functions [0, 1] → X . The initial configuration of the vehicle is xinit ∈ X . The path is required to end in a goal region Xgoal ⊆ X . The set of all allowed configurations of the vehicle is called the free configuration space and denoted Xfree . Typically, the free configurations are those that do not result in collision with obstacles, but the free-configuration set can also represent other holonomic constraints on the path. The differential constraints on the path are represented by a predicate D(x, x0 , x00 , . . .) and can be used to enforce some degree of smoothness of the path for the vehicle, such as the bound on the path curvature and/or

8

the rate of curvature. For example, in the case of X ⊆ R2 , the differential constraint may enforce the maximum curvature κ of the path using Frenet-Serret formula as follows: D(x, x0 , x00 , . . .) ⇔

kx0 × x00 k ≤ κ. kx0 k3

Further, let J(σ) : Σ(X ) → R be the cost functional. Then, the optimal version of the path planning problem can be generally stated as follows. Problem IV.1 (Optimal path planning). Given a 5-tuple (Xfree , xinit , Xgoal , D, J) find σ ∗ = arg min J(σ) σ∈Σ(X )

subj. to

σ(0) = xinit and σ(1) ∈ Xgoal σ(α) ∈ Xfree D(σ(α), σ 0 (α), σ 00 (α), . . .)

∀α ∈ [0, 1] ∀α ∈ [0, 1].

The problem of feasible and optimal path planning has been studied extensively in the past few decades. The complexity of this problem is well understood, and many practical algorithms have been developed. Complexity: A significant body of literature is devoted to studying the complexity of motion planning problems. The following is a brief survey of some of the major results regarding the computational complexity of these problems. The problem of finding an optimal path subject to holonomic and differential constraints as formulated in Problem IV.1 is known to be PSPACE-hard [64]. This means that it is at least as hard as solving any NP-complete problem and thus, assuming P 6= NP, there is no efficient (polynomialtime) algorithm able to solve all instances of the problem. Research attention has since been directed toward studying approximate methods, or approaches to subsets of the general motion planning problem. Initial research focused primarily on feasible (i.e., nonoptimal) path planning for a holonomic vehicle model in polygonal/polyhedral environments. That is, the obstacles are assumed to be polygons/polyhedra and there are no differential constraints on the resulting path. In 1970, Reif [64] found that an obstacle-free path for a holonomic vehicle, whose footprint can be described as a single polyhedron, can be found in polynomial time in both 2-D and 3-D environments. Canny [65] has shown that the problem of feasible path planning in a free space represented using polynomials is in PSPACE, which rendered the decision version of feasible path planning without differential constraints as a PSPACEcomplete problem. For the optimal planning formulation, where the objective is to find the shortest obstacle-free path. It has been long known that a shortest path for a holonomic vehicle in a 2D environment with polygonal obstacles can be found in polynomial time [66], [67]. More precisely, it can be computed in time O(n2 ), where n is the number of vertices of the polygonal obstacles [68]. This can be solved by constructing and searching the so-called visibility graph [69]. In contrast, Lazard, Reif and Wang [70] established that the problem of finding a shortest curvature-bounded path in a 2-D plane

amidst polygonal obstacles (i.e., a path for a car-like robot) is NP-hard, which suggests that there is no known polynomial time algorithm for finding a shortest path for a car-like robot among polygonal obstacles. A related result is that, that the existence of a curvature constrained path in polygonal environment can be decided in EXPTIME [71]. A special case where a solution can be efficiently computed is the shortest curvature bounded path in an obstacle free environment. Dubins [57] has shown that the shortest path having curvature bounded by κ between given two points p1 , p2 and with prescribed tangents θ1 , θ2 is a curve consisting of at most three segments, each one being either a circular arc segment or a straight line. Reeds and Shepp [58] extended the method for a car that can move both forwards and backwards. Another notable case due to Agraval et al. [72] is an O(n2 log n) algorithm for finding a shortest path with bounded curvature inside a convex polygon. Similarly, Boissonnat and Lazard [73] proposed a polynomial time algorithm for finding an exact curvature-bounded path in environments where obstacles have bounded-curvature boundary. Since for most problems of interest in autonomous driving, exact algorithms with practical computational complexity are unavailable [70], one has to resort to more general, numerical solution methods. These methods generally do not find an exact solution, but attempt to find a satisfactory solution or a sequence of feasible solutions that converge to the optimal solution. The utility and performance of these approaches are typically quantified by the class of problems for which they are applicable as well as their guarantees for converging to an optimal solution. The numerical methods for path planning can be broadly divided in three main categories: Variational methods represent the path as a function parametrized by a finite-dimensional vector and the optimal path is sought by optimizing over the vector parameter using non-linear continuous optimization techniques. These methods are attractive for their rapid convergence to locally optimal solutions; however, they typically lack the ability to find globally optimal solutions unless an appropriate initial guess in provided. For a detailed discussion on variational methods, see Section IV-C. Graph-search methods discretize the configuration space of the vehicle as a graph, where the vertices represent a finite collection of vehicle configurations and the edges represent transitions between vertices. The desired path is found by performing a search for a minimum-cost path in such a graph. Graph search methods are not prone to getting stuck in local minima, however, they are limited to optimize only over a finite set of paths, namely those that can be constructed from the atomic motion primitives in the graph. For a detailed discussion about graph search methods, see Section IV-D. Incremental search methods sample the configuration space and incrementally build a reachability graph (oftentimes a tree) that maintains a discrete set of reachable configurations and feasible transitions between them. Once the graph is large enough so that at least one node is in the goal region, the desired path is obtained by tracing the edges that lead to that node from the start configuration. In contrast to more basic graph search methods, sampling-based methods incrementally

9

increase the size of the graph until a satisfactory solution is found within the graph. For a detailed discussion about incremental search methods, see Section IV-E. Clearly, it is possible to exploit the advantages of each of these methods by combining them. For example, one can use a coarse graph search to obtain an initial guess for the variational method as reported in [74] and [75]. A comparison of key properties of select path planning methods is given in Table I. In the remainder of this section, we will discuss the path planning algorithms and their properties in detail. B. Trajectory Planning The motion planning problems in dynamic environments or with dynamic constraints may be more suitably formulated in the trajectory planning framework, in which the solution of the problem is a trajectory, i.e. a time-parametrized function π(t) : [0, T ] → X prescribing the evolution of the configuration of the vehicle in time. Let Π(X , T ) denote the set of all continuous functions [0, T ] → X and xinit ∈ X . be the initial configuration of the vehicle. The goal region is Xgoal ⊆ X . The set of all allowed configurations at time t ∈ [0, T ] is denoted as Xfree (t) and used to encode holonomic constraints such as the requirement on the path to avoid collisions with static and, possibly, dynamic obstacles. The differential constraints on the trajectory are represented by a predicate D(x, x0 , x00 , . . .) and can be used to enforce dynamic constraints on the trajectory. Further, let J(π) : Π(X , T ) → R be the cost functional. Under these assumptions, the optimal version of the trajectory planning problem can be very generally stated as: Problem IV.2 (Optimal trajectory planning). Given a 6-tuple (Xfree , xinit , Xgoal , D, J, T ) find π ∗ = arg min

J(π)

π∈Π(X ,T )

subj. to

π(0) = xinit and π(T ) ∈ Xgoal π(t) ∈ Xfree D(π(t), π 0 (t), π 00 (t), . . .)

∀t ∈ [0, T ] ∀t ∈ [0, T ].

Complexity: Since trajectory planning in a dynamic environment is a generalization of path planning in static environments, the problem remains PSPACE-hard. Moreover, trajectory planning in dynamic environments has been shown to be harder than path planning in the sense that some variants of the problem that are tractable in static environments become intractable when an analogical problem is considered in a dynamic environment. In particular, recall that a shortest path for a point robot in a static 2-D polygonal environment can be found efficiently in polynomial time and contrast it with the result of Canny and Reif [76] establishing that finding velocitybounded collision-free trajectory for a holonomic point robot amidst moving polygonal obstacles1 is NP-hard. Similarly, while path planning for a robot with a fixed number of degrees 1 In fact, the authors considered an even more constrained 2-D asteroid avoidance problem, where the task is to find a collision-free trajectory for a point robot with a bounded velocity in a 2-D plane with convex polygonal obstacles moving with a fixed linear speed.

of freedom in 3-D polyhedral environments is tractable, Reif and Sharir [85] established that trajectory planning for robot with 2 degrees of freedom among translating and rotating 3-D polyhedral obstacles is PSPACE-hard. Tractable exact algorithms are not available for non-trivial trajectory planning problems occurring in autonomous driving, making the numerical methods a popular choice for the task. Trajectory planning problems can be numerically solved using some variational methods directly in the time domain or by converting the trajectory planning problem to path planning in a configuration space with an added time-dimension [86]. The conversion from a trajectory planning problem T T (Xfree , xTinit , Xgoal , DT , J T , T ) to a path planning problem P P P (Xfree , xinit , Xgoal , DP , J P ) is usually done as follows. The configuration space where the path planning takes place is defined as X P := X T × [0, T ]. For any y ∈ X P , let t(y) ∈ [0, T ] denote the time component and c(y) ∈ X T denote the "configuration" component of the point y. A path σ(α) : [0, 1] → X P can be converted to a trajectory π(t) : [0, T ] → X T if the time component at start and end point of the path is constrained as t(σ(0)) = 0 t(σ(1)) = T and the path is monotonically-increasing, which can be enforced by a differential constraint t(σ 0 (α)) > 0

∀α ∈ [0, 1].

Further, the free configuration space, initial configuration, goal region and differential constraints is mapped to their path planning counterparts as follows: P Xfree = P xinit = P Xgoal = P 0 00 D (y, y , y , . . .) =

T {(x, t) : x ∈ Xfree (t) ∧ t ∈ [0, T ]} T (xinit , 0) T {(x, T ) : x ∈ Xgoal } 0 c(y ) c(y00 ) T D (c(y), t(y0 ) , t(y00 ) , . . .).

A solution to such a path planning problem is then found using a path planning algorithm that can handle differential constraints and converted back to the trajectory form.

C. Variational Methods We will first address the trajectory planning problem in the framework of non-linear continuous optimization. In this context, the problem is often referred to as trajectory optimization. Within this subsection we will adopt the trajectory planning formulation with the understanding that doing so does not affect generality since path planning can be formulated as trajectory optimization over the unit time interval. To leverage existing nonlinear optimization methods, it is necessary to project the infinite-dimensional function space of trajectories to a finite-dimensional vector space. In addition, most nonlinear programming techniques require the trajectory optimization problem, as formulated in Problem IV.2, to be

10

Model assumptions

Completeness

Optimality

Time Complexity

Anytime

Yes a

O(n2 ) [68] b

No

No

Exp. in dimension. [65]

No

Locally optimal

O(1/) [77] k,l

Yes

Geometric Methods

Cyl. algebr. decomp. [76]

2-D polyg. conf. space, no diff. constraints No diff. constraints

Variational methods (Sec IV-C)

Lipschitz-continuous Jacobian

Visibility graph [33]

Yes Yes Variational Methods No

Graph-search Methods Road lane graph + Dijkstra (Sec IV-D1) Lattice/tree of motion prim. + Dijkstra (Sec IV-E) PRM [79]

g

+ Dijkstra

Arbitrary

No c

No d

O(n + m log m) [78] e,f

No

Arbitrary

No c

No d

O(n + m log m) [78] e,f

No

O(n2 ) [80] h,f,∗

No

O(n log n) [80], [82] h,f,∗,†

No

O(n log n) [80] h,f,∗

Yes

O(n log n) [80] h,f,∗

Yes

O(n log n) [80], [82] h,f,∗,†

Yes

N/A j

Yes

Exact steering procedure available

PRM* [80], [81] + Dijkstra

Exact steering procedure available

RRG [80] + Dijkstra

Exact steering procedure available

RRT [83]

Arbitrary

RRT* [80]

Exact steering procedure available

SST* [84]

Lipschitz-continuous dynamics

Probabilistically Asymptotically complete [80] ∗ optimal ∗ [80] Probabilistically Asymptotically complete optimal [80]–[82] ∗,† [80]–[82] ∗,† Probabilistically Asymptotically complete [80] ∗ optimal [80] ∗ Incremental Search Probabilistically Suboptimal [80] ∗ complete [83] i,∗ Probabilistically Asymptotically complete [80], optimal [80], [82] ∗,† [82] ∗,† Probabilistically Asymptotically complete [84] † optimal [84] †

Table I: Comparison of path planning methods. Legend: a: for the shortest path problem; b: n is the number of points defining obstacles; c: complete only w.r.t. the set of paths induced by the given graph; d: optimal only w.r.t. the set of paths induced by the given graph; e: n and m are the number of edges and vertices in the graph respectively; f : assuming O(1) collision checking; g: batch version with fixed-radius connection strategy; h: n is the number of samples/algorithm iterations; i: for certain variants; j: not explicitly analyzed; k: is the required distance from the optimal cost; l: faster rates possible with additional assumptions; ∗: shown for systems without differential constraints; †: shown for some class of nonholonomic systems.

converted into the following form arg min

the form ˜ J(π) = J(π) + ε

J(π)

T

h(π(t))dt , 0

π∈Π(X ,T )

subj. to

Z

π(0) = xinit and π(T ) ∈ Xgoal f (π(t), π 0 (t), . . .) = 0 g(π(t), π 0 (t), . . .) ≤ 0

∀t ∈ [0, T ] ∀t ∈ [0, T ],

where the holonomic and differential constraints are represented as a system of equality and inequality constraints. In some applications the constrained optimization problem is relaxed to an unconstrained one using penalty or barrier functions. In both cases, the constraints are replaced by an augmented cost functional. With the penalty method, the cost functional takes the form Z 1 Th ˜ J(π) = J(π) + kf (π, π 0 , . . .)k2 + ε 0 i k max(0, g(π, π 0 , . . .))k2 dt . Similarly, barrier functions can be used in place of inequality constraints. The augmented cost functional in this case takes

where the barrier function satisfies g(π) < 0 ⇒ h(π) < ∞, g(π) ≥ 0 ⇒ h(π) = ∞, and limg(π)→0 {h(π)} = ∞. The intuition behind both of the augmented cost functionals is that, by making ε small, minima in cost will be close to minima of the original cost functional. An advantage of barrier functions is that local minima remain feasible, but must be initialized with a feasible solution to have finite augmented cost. Penalty methods on the other hand can be initialized with any trajectory and optimized to a local minima. However, local minima may violate the problem constraints. A variational formulation using barrier functions is proposed in [60] where a change of coordinates is used to convert the constraint that the vehicle remain on the road into a linear constraint. A logarithmic barrier is used with a Newton-like method in a similar fashion to interior point methods. The approach effectively computes minimum time trajectories for a detailed vehicle model over a segment of roadway.

11

Next, two subclasses of variational methods are discussed: Direct and indirect methods. Direct Methods: A general principle behind direct variational methods is to restrict the approximate solution to a finite-dimensional subspace of Π(X , T ). To this end, it is usually assumed that π(t) ≈ π ˜ (t) =

N X

πi φi (t),

i=1

where πi is a coefficient from R, and φi (t) are basis functions of the chosen subspace. A number of numerical approximation schemes have proven useful for representing the trajectory optimization problem as a nonlinear program. We mention here the two most common schemes: Numerical integrators with collocation and pseudospectral methods. 1) Numerical Integrators with Collocation: With collocation, it is required that the approximate trajectory satisfies the constraints in a set of discrete points {tj }M j=1 . This requirement results in two systems of discrete constraints: A system of nonlinear equations which approximates the system dynamics f (˜ π (tj ), π ˜ 0 (tj )) = 0 ∀j = 1, . . . , M and a system of nonlinear inequalities which approximates the state constraints placed on the trajectory g(˜ π (tj ), π ˜ 0 (tj )) ≤ 0

∀j = 1, . . . , M .

Numerical integration techniques are used to approximate the trajectory between the collocation points. For example, a piecewise linear basis (t − ti−1 )/(ti − ti−1 ) if t ∈ [ti−1 , ti ] (ti+1 − t)/(ti+1 − ti ) if t ∈ [ti , ti+1 ] φi (t) = 0 otherwise together with collocation gives rise to the Euler integration method. Higher order polynomials result in the Runge-Kutta family of integration methods. Formulating the nonlinear program with collocation and Euler’s method or one of the RungeKutta methods is more straightforward than some other methods making it a popular choice. An experimental system which successfully uses Euler’s method for numerical approximation of the trajectory is presented in [14]. In contrast to Euler’s method, the Adams approximation, is investigated in [87] for optimizing the trajectory for a detailed vehicle model and is shown to provide improved numerical accuracy and convergence rates. 2) Pseudospectral Methods: Numerical integration techniques utilize a discretization of the time interval with an interpolating function between collocation points. Pseudospectral approximation schemes build on this technique by additionally representing the interpolating function with a basis. Typical basis functions interpolating between collocation points are finite subsets of the Legendre or Chebyshev polynomials. These methods typically have improved convergence rates over basic collocation methods, which is especially true when adaptive methods for selecting collocation points and basis functions are used as in [88].

Indirect Methods: Pontryagin’s minimum principle [89], is a celebrated result from optimal control which provides optimality conditions of a solution to Problem IV.2. Indirect methods, as the name suggests, solve the problem by finding solutions satisfying these optimality conditions. These optimality conditions are described as an augmented system of ordinary differential equations (ODEs) governing the states and a set of co-states. However, this system of ODEs results in a two point boundary value problem and can be difficult to solve numerically. One technique is to vary the free initial conditions of the problem and integrate the system forward in search of the initial conditions which leads to the desired terminal states. This method is known as the shooting method, and a version of this approach has been applied to planning parking maneuvers in [90]. The advantage of indirect methods, as in the case of the shooting method, is the reduction in dimensionality of the optimization problem to the dimension of the state space. The topic of variational approaches is very extensive and hence, the above is only a brief description of select approaches. See [91], [92] for dedicated surveys on this topic. D. Graph Search Methods Although useful in many contexts, the applicability of variational methods is limited by their convergence to only local minima. In this section, we will discuss the class of methods that attempts to mitigate the problem by performing global search in the discretized version of the path space. These socalled graph search methods discretize the configuration space X of the vehicle and represent it in the form of a graph and then search for a minimum cost path on such a graph. In this approach, the configuration space is represented as a graph G = (V, E), where V ⊂ X is a discrete set of selected configurations called vertices and E = {(oi , di , σi )} is the set of edges, where oi ∈ V represents the origin of the edge, di represents the destination of the edge and σi represents the path segments connecting oi and di . It is assumed that the path segment σi connects the two vertices: σi (0) = oi and σi (1) = di . Further, it is assumed that the initial configuration xinit is a vertex of the graph. The edges are constructed in such a way that the path segments associated with them lie completely in Xfree and satisfy differential constraints. As a result, any path on the graph can be converted to a feasible path for the vehicle by concatenating the path segments associated with edges of the path through the graph. There is a number of strategies for constructing a graph discretizing the free configuration space of a vehicle. In the following subsections, we discuss three common strategies: Hand-crafted lane graphs, graphs derived from geometric representations and graphs constructed by either control or configuration sampling. 1) Lane Graph: When the path planning problem involves driving on a structured road network, a sufficient graph discretization may consist of edges representing the path that the car should follow within each lane and paths that traverse intersections. Road lane graphs are often partly algorithmically generated

12

from higher-level street network maps and partly human edited. An example of such a graph is in Figure IV.1.

Figure IV.1: Hand-crafted graph representing desired driving paths under normal circumstances. Although most of the time it is sufficient for the autonomous vehicle to follow the paths encoded in the road lane graph, occasionally it must be able to navigate around obstacles that were not considered when the road network graph was designed or in environments not covered by the graph. Consider for example a faulty vehicle blocking the lane that the vehicle plans to traverse – in such a situation a more general motion planning approach must be used to find a collision-free path around the detected obstacle. The general path planning approaches can be broadly divided into two categories based on how they represent the obstacles in the environment. So-called geometric or combinatorial methods work with geometric representations of the obstacles, where in practice the obstacles are most commonly described using polygons or polyhedra. On the other hand, socalled sampling-based methods abstract away from how the obstacles are internally represented and only assumes access to a function that determines if any given path segment is in collision with any of the obstacles. 2) Geometric Methods: In this section, we will focus on path planning methods that work with geometric representations of obstacles. We will first concentrate on path planning without differential constraints because for this formulation, efficient exact path planning algorithms exist. Although not being able to enforce differential constraints is limiting for path planning for traditionally-steered cars because the constraint on minimum turn radius cannot be accounted for, these methods can be useful for obtaining the lower- and upperbounds2 on the length of a curvature-constrained path and for path planning for more exotic car constructions that can turn on the spot. In path planning, the term roadmap is used to describe a graph discretization of Xfree that describes well the connectivity of the free configuration space and has the property that any point in Xfree is trivially reachable from some vertices of the roadmap. When the set Xfree can be described geometrically 2 Lower bound is the length of the path without curvature constraint, upper bound is the length of a path for a large robot that serves as an envelope within which the car can turn in any direction.

using a linear or semi-algebraic model, different types of roadmaps for Xfree can be algorithmically constructed and subsequently used to obtain complete path planning algorithms. Most notably, for Xfree ⊆ R2 and polygonal models of the configuration space, several efficient algorithms for constructing such roadmaps exists such as the vertical cell decomposition [93], generalized Voronoi diagrams [94], [95], and visibility graphs [33], [96]. For higher dimensional configuration spaces described by a general semi-algebraic model, the technique known as cylindrical algebraic decomposition can be used to construct a roadmap in the configuration space [47], [97] leading to complete algorithms for a very general class of path planning problems. The fastest of this class is an algorithm developed by Canny [65] that has (single) exponential time complexity in the dimension of the configuration space. The result is however mostly of a theoretical nature without any known implementation to date. Due to its relevance to path planning for car-like vehicles, a number of results also exist for the problem of path planning with a constraint on maximum curvature. Backer and Kirkpatrick [98] provide an algorithm for constructing a path with bounded curvature that is polynomial in the number of features of the domain, the precision of the input and the number of segments on the simplest obstacle-free Dubins path connecting the specified configurations. Since the problem of finding a shortest path with bounded curvature amidst polygonal obstacles is NP-hard, it is not surprising that no exact polynomial solution algorithm is known. An approximation algorithm for finding shortest curvature-bounded path amidst polygonal obstacles has been first proposed by Jacobs and Canny [99] and later improved by Wang and Agarwal [100] 2 with time complexity O( n4 log n), where n is the number of vertices of the obstacles and is the approximation factor. For the special case of so-called moderate obstacles that are characterized by smooth boundary with curvature bounded by κ, an exact polynomial algorithm for finding a path with curvature bounded by at most κ have been developed by Boissonnat and Lazard [73]. 3) Sampling-based Methods: In autonomous driving, a geometric model of Xfree is usually not directly available and it would be too costly to construct from raw sensoric data. Moreover, the requirements on the resulting path are often far more complicated than a simple maximum curvature constraint. This may explain the popularity of sampling-based techniques that do not enforce a specific representation of the free configuration set and dynamic constraints. Instead of reasoning over a geometric representation, the sampling based methods explore the reachability of the free configuration space using steering and collision checking routines: The steering function steer(x, y) returns a path segment starting from configuration x going towards configuration y (but not necessarily reaching y) ensuring the differential constraints are satisfied, i.e., the resulting motion is feasible for the vehicle model in consideration. The exact manner in which the steering function is implemented depends on the context in which it is used. Some typical choices encountered in the literature are: 1) Random steering: The function returns a path that results

13

from applying a random control input through a forward model of the vehicle from state x for either a fixed or variable time step [101]. 2) Heuristic steering: The function returns a path that results from applying control that is heuristically constructed to guide the system from x towards y [102]– [104]. This includes selecting the maneuver from a predesigned discrete set (library) of maneuvers. 3) Exact steering: The function returns a feasible path that guides the system from x to y. Such a path corresponds to a solution of a 2-point boundary value problem. For some systems and cost functionals, such a path can be obtained analytically, e.g., a straight line for holonomic systems, a Dubins curve for forward-moving unicycle [57], or a Reeds-Shepp curve for bi-directional unicycle [58]. An analytic solution also exists for differentially flat systems [61], while for more complicated models, the exact steering can be obtained by solving the twopoint boundary value problem. 4) Optimal exact steering: The function returns an optimal exact steering path with respect to the given cost functional. In fact, the straight line, the Dubins curve, and the Reeds-Shepp curve from the previous point are optimal solutions assuming that the cost functional is the arc-length of the path [57], [58]. The collision checking function col-free(σ) returns true if path segment σ lies entirely in Xfree and it is used to ensure that the resulting path does not collide with any of the obstacles. Having access to steering and collision checking functions, the major challenge becomes how to construct a discretization that approximates well the connectivity of Xfree without having access to an explicit model of its geometry. We will now review sampling-based discretization strategies from literature. A straightforward approach is to choose a set of motion primitives (fixed maneuvers) and generate the search graph by recursively applying them starting from the vehicle’s initial configuration xinit , e.g., using the method in Algorithm 1. For path planning without differential constraints, the motion primitives can be simply a set of straight lines with different directions and lengths. For a car-like vehicle, such motion primitive might by a set of arcs representing the path the car would follow with different values of steering. A variety of techniques can be used for generating motion primitives for driverless vehicles. A simple approach is to sample a number of control inputs and to simulate forwards in time using a vehicle model to obtain feasible motions. In the interest of having continuous curvature paths, clothoid segments are also sometimes used [105]. The motion primitives can be also obtained by recording the motion of a vehicle driven by an expert driver [106]. Observe that the recursive application of motion primitives may generate a tree graph in which in the worst-case no two edges lead to the same configuration. There are, however, sets of motion primitives, referred to as lattice-generating, that result in regular graphs resembling a lattice. See Figure IV.2a for an illustration. The advantage of lattice generating primitives is that the vertices of the search graph cover the configuration

space uniformly, while trees in general may have a high density of vertices around the root vertex. Pivtoraiko et al. use the term "state lattice" to describe such graphs in [107] and point out that a set of lattice-generating motion primitives for a system in hand can be obtained by first generating regularly spaced configurations around origin and then connecting the origin to such configurations by a path that represents the solution to the two-point boundary value problem between the two configurations. Algorithm 1: Recursive Roadmap Construction V ← {xinit }; E ← ∅; Q ← new queue(xinit ); while Q 6= ∅ do x ← pop element from Q; M ← generate a set of path segments by applying motion primitives from configuration x; for σ ∈ M do if col-free(σ) then E ← E ∪ {(x, σ(1), σ)}; if σ(1) 6∈ V then add σ(1) to Q; V ← V ∪ {σ(1)}; return (V,E) An effect that is similar to recursive application of latticegenerating motion primitives from the initial configuration can be achieved by generating a discrete set of samples covering the (free) configuration space and connecting them by feasible path segments obtained using an exact steering procedure. Most sampling-based roadmap construction approaches follow the algorithmic scheme shown in Algorithm 2, but differ in the implementation of the sample-points(X , n) and neighbors(x, V ) routines. The function samplepoints(X , n) represents the strategy for selecting n points from the configuration space X , while the function neighbors(x, V ) represents the strategy for selecting a set of neighboring vertices N ⊆ V for a vertex x, which the algorithm will attempt to connect to x by a path segment using an exact steering function, steerexact (x, y). Algorithm 2: Sampling-based Roadmap Construction V ← {xinit } ∪ sample-points(X , n); E ← ∅; for x ∈ V do for y ∈ neighbors(x, V ) do σ ← steerexact (x, y); if col-free(σ) then E ← E ∪ {(x, y, σ)}; return (V,E) The two most common implementations of samplepoints(X , n) function are 1) return n points arranged in a regular grid and 2) return n randomly sampled points from X . While random sampling has an advantage of being generally applicable and easy to implement, so-called Sukharev grids

14

(a) Lattice graph

(b) Non-lattice graph

Figure IV.2: Lattice and non-lattice graph, both with 5000 edges. (a) The graph resulting from recursive application of 90◦ left circular arc, 90◦ right circular arc, and a straight line. (b) The graph resulting from recursive application of 89◦ left circular arc, 89◦ right circular arc, and a straight line. The recursive application of those primitives does form a tree instead of a lattice with many branches looping in the neighborhood of the origin. As a consequence, the area covered by the right graph is smaller.

have been shown to achieve optimal L∞ -dispersion in unit hypercubes, i.e. they minimize the radius of the largest empty ball with no sample point inside. For in depth discussion of relative merits of random and deterministic sampling in the context of sampling-based path planning, we refer the reader to [108]. The two most commonly used strategies for implementing neighbors(x, V ) function are to take 1) the set of k-nearest neighbors to x or 2) the set of points lying within the ball centered at x with radius r. In particular, samples arranged deterministically in a ddimensional grid with the neighborhood taken as 4 or 8 nearest neighbors in 2-D or the analogous pattern in higher dimensions represents a straightforward deterministic discretization of the free configuration space. This is in part because they arise naturally from widely used bitmap representations of free and occupied regions of robots’ configuration space [109]. Kavraki et al. [110] advocate the use of random sampling within the framework of Probabilistic Roadmaps (PRM) in order to construct roadmaps in high-dimensional configuration spaces, because unlike grids, they can be naturally run in an anytime fashion. The batch version of PRM [79] follows the scheme in Algorithm 2 with random sampling and neighbors selected within a ball with fixed radius r. Due to the general formulation of PRMs, they have been used for path planning for a variety of systems, including systems with differential constraints. However, the theoretical analyses of the algorithm have primarily been focused on the performance of the algorithm for systems without differential constraints, i.e. when a straight line is used to connect two configurations. Under such an assumption, PRMs have been shown in [80] to be probabilistically complete and asymptotically optimal. That is, the probability that the resulting graph contains a valid solution (if it exists) converges to one with increasing size of the graph and the cost of the shortest path in the graph converges to the optimal cost. Karaman and Frazzoli [80] proposed an adapta-

tion of batch PRM, called PRM*, that instead only connects neighboring vertices in a ball with a logarithmically shrinking radius with increasing number of samples to maintain both asymptotic optimality and computational efficiency. In the same paper, the authors propose Rapidly-exploring Random Graphs (RRG*), which is an incremental discretization strategy that can be terminated at any time while maintaining the asymptotic optimality property. Recently, Fast Marching Tree (FMT*) [111] has been proposed as an asymptotically optimal alternative to PRM*. The algorithm combines discretization and search into one process by performing a lazy dynamic programming recursion over a set of sampled vertices that can be subsequently used to quickly determine the path from initial configuration to the goal region. Recently, the theoretical analysis has been extended also to differentially constrained systems. Schmerling et al. [81] propose differential versions of PRM* and FMT* and prove asymptotic optimality of the algorithms for driftless controlaffine dynamical systems, a class that includes models of nonslipping wheeled vehicles. 4) Graph Search Strategies: In the previous section, we have discussed techniques for the discretization of the free configuration space in the form of a graph. To obtain an actual optimal path in such a discretization, one must employ one of the graph search algorithms. In this section, we are going to review the graph search algorithms that are relevant for path planning. The most widely recognized algorithm for finding shortest paths in a graph is probably the Dijkstra’s algorithm [32]. The algorithm performs the best first search to build a tree representing shortest paths from a given source vertex to all other vertices in the graph. When only a path to a single vertex is required, a heuristic can be used to guide the search process. The most prominent heuristic search algorithm is A* developed by Hart, Nilsson and Raphael [112]. If the provided

15

heuristic function is admissible (i.e., it never overestimates the cost-to-go), A* has been shown to be optimally efficient and is guaranteed to return an optimal solution. For many problems, a bounded suboptimal solution can be obtained with less computational effort using Weighted A* [113], which corresponds to simply multiplying the heuristic by a constant factor > 1. It can be shown that the solution path returned by A* with such an inflated heuristics is guaranteed to be no worse than (1 + ) times the cost of an optimal path. Often, the shortest path from the vehicle’s current configuration to the goal region is sought repeatedly every time the model of the world is updated using sensory data. Since each such update usually affects only a minor part of the graph, it might be wasteful to run the search every time completely from scratch. The family of real-time replanning search algorithms such as D* [114], Focussed D* [115] and D* Lite [116] has been designed to efficiently recompute the shortest path every time the underlying graph changes, while making use of the information from previous search efforts. Anytime search algorithms attempt to provide a first suboptimal path quickly and continually improve the solution with more computational time. Anytime A* [117] uses a weighted heuristic to find the first solution and achieves the anytime behavior by continuing the search with the cost of the first path as an upper bound and the admissible heuristic as a lower bound, whereas Anytime Repairing A* (ARA*) [118] performs a series of searches with inflated heuristic with decreasing weight and reuses information from previous iterations. On the other hand, Anytime Dynamic A* (ADA*) [119] combines ideas behind D* Lite and ARA* to produce an anytime search algorithm for real-time replanning in dynamic environments. A clear limitation of algorithms that search for a path on a graph discretization of the configuration space is that the resulting optimal path on such graph may be significantly longer than the true shortest path in the configuration space. Any-angle path planning algorithms [120]–[122] are designed to operate on grids, or more generally on graphs representing cell decomposition of the free configuration space, and try to mitigate this shortcoming by considering "shortcuts" between the vertices on the graph during search. In addition, Field D* introduces linear-interpolation to the search procedure to produce smooth paths [123]. E. Incremental Search Techniques A disadvantage of the techniques that search over a fixed graph discretization is that they search only over the set of paths that can be constructed from primitives in the graph discretization. Therefore, these techniques may fail to return a feasible path or return a noticeably suboptimal one. The incremental feasible motion planners strive to address this problem and provide a feasible path to any motion planning problem instance, if one exists, given enough computation time. Typically, these methods incrementally build increasingly finer discretization of the configuration space while concurrently attempting to determine if a path from initial configuration to the goal region exists in the discretization at each step. If the instance is “easy”, the solution is

provided quickly, but in general the computation time can be unbounded. Similarly, incremental optimal motion planning approaches on top of finding a feasible path fast attempt to provide a sequence of solutions of increasing quality that converges to an optimal path. The term probabilistically complete is used in the literature to describe algorithms that find a solution, if one exists, with probability approaching one with increasing computation time. Note that probabilistically complete algorithm may not terminate if the solution does not exist. Similarly, the term asymptotically optimal is used for algorithms that converge to optimal solution with probability one. A naïve strategy for obtaining completeness and optimality in the limit is to solve a sequence of path planning problems on a fixed discretization of the configuration space, each time with a higher resolution of the discretization. One disadvantage of this approach is that the path planning processes on individual resolution levels are independent without any information reuse. Moreover, it is not obvious how fast the resolution of the discretization should be increased before a new graph search is initiated, i.e., if it is more appropriate to add a single new configuration, double the number of configuration, or double the number of discrete values along each configuration space dimension. To overcome such issues, incremental motion planning methods interweave incremental discretization of configuration space with search for a path within one integrated process. An important class of methods for incremental path planning is based on the idea of incrementally growing a tree rooted at the initial configuration of the vehicle outwards to explore the reachable configuration space. The "exploratory" behavior is achieved by iteratively selecting a random vertex from the tree and by expanding the selected vertex by applying the steering function from it. Once the tree grows large enough to reach the goal region, the resulting path is recovered by tracing the links from the vertex in the goal region backwards to the initial configuration. The general algorithmic scheme of an incremental tree-based algorithm is described in Algorithm 3. Algorithm 3: Incremental Tree-based Algorithm V ← {xinit } ∪ sample-points(X , n); E ← ∅; while not interrupted do xselected ←select(V ); σ ←extend(xselected , V ); if col-free(σ) then xnew = σ(1); V ← V ∪{xnew }; E ← E ∪{(xselected , xnew , σ)}; return (V,E) One of the first randomized tree-based incremental planners was the expansive spaces tree (EST) planner proposed by Hsu et al. [124]. The algorithm selects a vertex for expansion, xselected , randomly from V with a probability that is inversely proportional to the number of vertices in its neighborhood, which promotes growth towards unexplored regions. During expansion, the algorithm samples a new vertex y within a

16

neighborhood of a fixed radius around xselected , and use the same technique for biasing the sampling procedure to select a vertex from the region that is relatively less explored. Then it returns a straight line path between xselected and y. A generalization of the idea for planning with kinodynamic constraints in dynamic environments was introduced in [125], where the capabilities of the algorithm were demonstrated on different non-holonomic robotic systems and the authors use an idealized version of the algorithm to establish that the probability of failure to find a feasible path depends on the expansiveness property of the state space and decays exponentially with the number of samples. Rapidly-exploring Random Trees (RRT) [101] have been proposed by La Valle as an efficient method for finding feasible trajectories for high-dimensional non-holonomic systems. The rapid exploration is achieved by taking a random sample xrnd from the free configuration space and extending the tree in the direction of the random sample. In RRT, the vertex selection function select(V) returns the nearest neighbor to the random sample xrnd according to the given distance metric between the two configurations. The extension function extend() then generates a path in the configuration space by applying a control for a fixed time step that minimizes the distance to xrnd . Under certain simplifying assumptions (random steering is used for extension), the RRT algorithm has been shown to be probabilistic complete [83]. We remark that the result on probabilistic completeness does not readily generalize to many practically implemented versions of RRT that often use heuristic steering. In fact, it has been recently shown in [126] that RRT using heuristic steering with fixed time step is not probabilistically complete. Moreover, Karaman and Frazzoli [127] demonstrated that the RRT converges to a suboptimal solution with probability one and designed an asymptotically optimal adaptation of the RRT algorithm, called RRT*. As shown in Algorithm 4, the RRT* at every iteration considers a set of vertices that lie in the neighborhood of newly added vertex xnew and a) connects xnew to the vertex in the neighborhood that minimizes the cost of path from xinit to xnew and b) rewires any vertex in the neighborhood to xnew if that results in a lower cost path from xinit to that vertex. An important characteristic of the algorithm is that the neighborhood region is defined as the ball centered at xnew p with radius being function of the size of the tree: r = γ d (log n)/n, where n is the number of vertices in the tree, d is the dimension of the configuration space, and γ is an instance-dependent constant. It is shown that for such a function, the expected number of vertices in the ball is logarithmic in the size of the tree, which is necessary to ensure that the algorithm almost surely converges to an optimal path while maintaining the same asymptotic complexity as the suboptimal RRT. Sufficient conditions for asymptotic optimality of RRT* under differential constraints are stated in [80] and demonstrated to be satisfiable for Dubins vehicle and double integrator systems. In a later work, the authors further show in the context of small-time locally attainable systems that the algorithm can be adapted to maintain not only asymptotic optimality, but also computational efficiency [82]. Other related works focus

Algorithm 4: RRT* Algorithm. The cost-to-come to vertex x is denoted as c(x), the cost of path segment σ is denoted as c(σ) and the parent vertex of vertex x is denoted by p(x). V ← {xinit }; E ← ∅; while not interrupted do xselected ←select(V ); σ ←extend(xselected , V ); if col-free(σ) then xnew = σ(1) ; V ← V ∪ {xnew }; // consider all vertices in ball of q radius r around xnew r = γ d log|V|V| | ; Xnear ← {x ∈ V \ {xnew } : d(xnew , x} < r}; // find best parent xpar ← arg minx∈Xnear c(x) + c(connect(x, xnew )) subj. to col-free(steerexact (x, xnew )); σ 0 = steerexact (xpar , xnew ); E ← E ∪ {(xpar , xnew , σ 0 )}; // rewire vertices in neighborhood for x ∈ Xnear do σ 0 ← steerexact (xnew , x); if c(xnew ) + c(σ 0 ) < c(x) and col-free(σ 0 ) then E ← (E \ {(p(x), x, σ 00 )}) ∪ {(xnew , x, σ 0 )}, where σ 00 is the path from p(x) to x; return (V,E)

on deriving distance and steering functions for non-holonomic systems by locally linearizing the system dynamics [128] or by deriving a closed-form solution for systems with linear dynamics [129]. On the other hand, RRTX is an algorithm that extends RRT∗ to allow for real-time incremental replanning when the obstacle region changes, e.g., in the face of new data from sensors [130]. New developments in the field of sampling-based algorithms include algorithms that achieve asymptotic optimality without having access to an exact steering procedure. In particular, Li at al. [84] recently proposed the Stable Sparse Tree (SST) method for asymptotically (near-)optimal path planning, which is based on building a tree of randomly sampled controls propagated through a forward model of the dynamics of the system such that the locally suboptimal branches are pruned out to ensure that the tree remains sparse. F. Practical Deployments Three categories of path planning methodologies have been discussed for self-driving vehicles: variational methods, graph-

17

searched methods and incremental tree-based methods. The actual field-deployed algorithms on self-driving systems come from all the categories described above. For example, even among the first four successful participants of DARPA Urban Challenge, the approaches used for motion planning significantly differed. The winner of the challenge, CMU’s Boss vehicle used variational techniques for local trajectory generation in structured environments and a lattice graph in 4-dimensional configuration space (consisting of position, orientation, and velocity) together with Anytime D* to find a collision-free paths in parking lots [131]. The runner-up vehicle developed by Stanford’s team reportedly used a search strategy coined Hybrid A* that during search, lazily constructs a tree of motion primitives by recursively applying a finite set of maneuvers. The search is guided by a carefully designed heuristic and the sparsity of the tree is ensured by only keeping a single node within a given region of the configuration space [132]. Similarly, the vehicle arriving third developed by the VictorTango team from Virginia Tech constructs a graph discretization of possible maneuvers and searches the graph with the A* algorithm [133]. Finally, the vehicle developed by MIT used a variant of RRT algorithm called closed-loop RRT with biased sampling [134]. V. V EHICLE C ONTROL Solutions to Problem IV.1 or IV.2 are provided by the motion planning process. The role of the feedback controller is to stabilize to the reference path or trajectory in the presence of modeling error and other forms of uncertainty. Depending on the reference provided by the motion planner, the control objective may be path stabilization or trajectory stabilization. More formally, the path stabilization problem is stated as follows: Problem V.1. (Path stabilization) Given a controlled differential equation x˙ = f (x, u), reference path xref : R → Rn , and velocity vref : R → R, find a feedback law, u(x), such that solutions to x˙ = f (x, u(x)) satisfy the following: ∀ε > 0 and t1 < t2 , there exists a δ > 0 and a differentiable s : R → R such that 1) kx(t1 ) − xref (s(t1 ))k ≤ δ ⇒ kx(t2 ) − xref (s(t2 ))k ≤ ε 2) limt→∞ kx(t) − xref (s(t))k = 0 3) limt→∞ s(t) ˙ = vref (s(t)). Qualitatively, these conditions are that (1) a small initial tracking error will remain small, (2) the tracking error must converge to zero, and (3) progress along the reference path tends to a nominal rate. Many of the proposed vehicle control laws, including several discussed in this section, use a feedback law of the form u(x) = f argminkx − xref (γ)k , (V.1) γ

where the feedback is a function of the nearest point on the reference path. An important issue with controls of this form is that the closed loop vector field f (x, u(x)) will not be continuous. If the path is self intersecting or not differentiable

at some point, a discontinuity in which f (x, u(x)) is will lie directly on the path. This leads to unpredictable behavior if the executed trajectory encounters the discontinuity. This discontinuity is illustrated in Figure V.1. A backstepping control design which does not use a feedback law of the form (V.1) is presented in [135].

Figure V.1: Visualization of (V.5) for a sample reference path shown in black. The color indicates the value of s for each point in the plane and illustrates discontinuities in (V.5). The trajectory stabilization problem is more straightforward, but these controllers are prone to performance limitations [136]. Problem V.2. (Trajectory stabilization) Given a controlled differential equation x˙ = f (x, u) and a reference trajectory xref (t), find π(x) such that solutions to x˙ = f (x, π(x)) satisfy the following: ∀ε > 0 and t1 < t2 , there exists a δ > 0 such that 1) kx(t1 ) − xref (t1 )k ≤ δ ⇒ kx(t2 ) − xref (t2 )k ≤ ε 2) limt→∞ kx(t) − xref (t)k = 0 In many cases, analyzing the stability of trajectories can be reduced to determining the origin’s stability in a time varying system. The basic form of Lyapunov’s theorem is only applicable to time invariant systems. However, stability theory for time varying systems is also well established (e.g. [137, Theorem 4.9]). Some useful qualifiers for various types of stability include: • Uniform asymptotic stability for a time varying system which asserts that δ in condition 1 of the above problem is independent of t1 . • Exponential stability asserts that the rate of convergence is bounded above by an exponential decay. A delicate issue that should be noted is that controller specifications are usually expressed in terms of the asymptotic tracking error as time tends to infinity. In practice, reference trajectories are finite so there should also be consideration for the transient response of the system. The remainder of this section is devoted to a survey of select control designs which are applicable to driverless cars. An overview of these controllers is provided in Table II.

18

Controller Pure Pursuit Rear wheel based feedback Front wheel based feedback Feedback linearization Control Lyapunov design

Model (V-A1)

Kinematic

(V-A2)

Kinematic

(V-A3)

Kinematic

(V-B2)

Steering rate controlled kinematic

(V-B1)

Kinematic

Linear MPC

(V-C)

Nonlinear MPC

(V-C)

C 1 (Rn × Rm ) model] 1 C (Rn × Rm ) model]

Stability LES? to ref. path LES? to ref. path LES? to ref. path LES? to ref. traj. LES? to ref. traj. LES? to ref. or path Not guaranteed

Time Complexity

Comments/Assumptions

O(n)∗

No path curvature

O(n)∗

C 2 (Rn ) ref. paths

O(n)∗ O(1) O(1) O

√

N ln

O( 1ε ) ‡

N ε

†

C 1 (Rn ) ref. paths; Forward driving only C 1 (Rn ) ref. traj.; Forward driving only Stable for constant path curvature and velocity Stability depends on horizon length Works well in practice

Table II: Overview of controllers discussed within this section. Legend: ?: local exponential stability (LES); ∗: assuming (V.1) is evaluated by a linear search over an n-point discretization of the path or trajectory; †: assuming the use of an interior-point method to solve (V.28) with a time horizon of n and solution accuracy of ε; ‡: based on asymptotic convergence rate to local minimum of (V.25) using steepest descent. Not guaranteed to return solution or find global minimum.; ]: vector field over the state space Rn defined by each input in Rm is a continuously differentiable function so that the gradient of the cost or linearization about the reference is defined.

Subsection V-A details a number of effective control strategies for path stabilization of the kinematic model, and subsection V-B2 discusses trajectory stabilization techniques. Predictive control strategies, discussed in subsection V-C, are effective for more complex vehicle models and can be applied to path and trajectory stabilization. A. Path Stabilization for the Kinematic Model 1) Pure Pursuit: Among the earliest proposed path tracking strategies is pure pursuit. The first discussion appeared in [138], and was elaborated upon in [52], [139]. This strategy and its variations (e.g. [140], [141]) have proven to be an indispensable tool for vehicle control owing to its simple implementation and satisfactory performance. Numerous publications including two vehicles in the DARPA Grand Challenge [8] and three vehicles in the DARPA Urban challenge [9] reported using the pure pursuit controller. The control law is based on fitting a semi-circle through the vehicle’s current configuration to a point on the reference path ahead of the vehicle by a distance L called the lookahead distance. Figure V.2 illustrates the geometry. The circle is defined as passing through the position of the car and the point on the path ahead of the car by one lookahead distance with the circle tangent to the car’s heading. The curvature of the circle is given by 2 sin(α) . (V.2) L For a vehicle speed vr , the commanded heading rate is κ=

2vr sin(α) . (V.3) L In the original publication of this controller [138], the angle α is computed directly from camera output data. However, α ω=

Figure V.2: Geometry of the pure pursuit controller. A circle (blue) is fit between rear wheel position and the reference path (brown) such that the chord length (green) is the look ahead distance L and the circle is tangent to the current heading direction.

can be expressed in terms of the inertial coordinate system to define a state feedback control. Consider the configuration (xr , yr , θ)T and the points on the path, (xref (s), yref (s)), such that k(xref (s), yref (s)) − (xr , yr )k = L. Since there is generally more than one such point on the reference, take the one with the greatest value of the parameter s to uniquely define a control. Then α is given by yref − yr − θ. (V.4) α = arctan xref − xr Assuming that the path has no curvature and the vehicle speed is constant (potentially negative), the pure pursuit controller solves Problem V.1. For a fixed nonzero curvature, pure pursuit has a small steady state tracking error. In the case where the vehicle’s distance to the path is greater than L, the controller output is not defined. Another consideration is that changes in reference path curvature can lead to

19

the car deviating from the reference trajectory. This may be acceptable for driving along a road, but can be problematic for tracking parking maneuvers. Lastly, the heading rate command ω becomes increasingly sensitive to the feedback angle α as the vehicle speed increases. A common fix for this issue is to scale L with the vehicle speed. 2) Rear wheel position based feedback: The next approach uses the rear wheel position as an output to stabilize a nominal rear wheel path [56]. The controller assigns s(t) = argmink(xr (t), yr (t)) − (xref (γ), yref (γ))k. (V.5) γ

Detailed assumptions on the reference path and a finite domain containing the reference path where (V.5) is a continuous function are described in [56]. The unit tangent to the path at s(t) is given by ∂yref ∂xref , ∂s ∂s s(t) s(t) ˆ

, (V.6) t= ∂xref (s(t)) ∂yref (s(t)) ,

∂s ∂s and the tracking error vector is d(t) := (xr (t), yr (t)) − (xref (s(t)), yref (s(t))) .

(V.7)

These values are used to compute a transverse error coordinate from the path e which is a cross product between the two vectors e = dx tˆy − dy tˆx (V.8) with the subscript denoting the component indices of the vector. The control uses the angle θe between the vehicle’s heading vector and the tangent vector to the path. ∂yref (s(t)) ∂xref (s(t)) , . (V.9) θe (t) = θ − arctan2 ∂s ∂s The geometry is illustrated in Figure V.3.

A change of coordinates to (s, e, θe ) yields = = =

vr cos(θe ) 1−κ(s)e ,

vr sin(θe ), cos(θe ) ω − vr κ(s) , 1−κ(s)e

ω=

(V.10)

sin(θe ) vr κ(s) cos(θe ) − g1 (e, θe , t)θe − k2 vr e, (V.11) 1 − κ(s)e θe

with g1 (e, θe , t) > 0, k2 > 0, and vr 6= 0 which is verified with the Lyapunov function V (e, θe ) = e2 + θe2 /k2 in [56] using the coordinate system (V.10). The requirement that the path be twice differentiable comes from the appearance of the curvature in the feedback law. An advantage of this control law is that stability is unaffected by the sign of vr making it suitable for reverse driving. Setting g1 (vr , θe , t) = kθ |vr | for ke > 0 leads to local exponential convergence with a rate independent of the vehicle speed so long as vr 6= 0. The control law in this case is sin(θe ) vr κ(s) cos(θe ) −(kθ |vr |) θe − ke vr e. (V.12) ω= 1 − κ(s)e θe 3) Front wheel position based feedback: This approach was proposed and used in Stanford University’s entry to the 2005 DARPA Grand Challenge [55], [8]. The approach is to take the front wheel position as the regulated variable. The control uses the variables s(t), e(t), and θe (t) as in the previous subsections, with the modification that e(t) is computed with the front wheel position as opposed to the rear wheel position. Taking the time derivative of the transverse error reveals e˙ = vf sin (θe + δ)

(V.13)

The error rate in (V.13) can be directly controlled by the steering angle for error rates with magnitude less than vf . Solving for the steering angle such that e˙ = −ke drives e(t) to zero exponentially fast. ⇒

Figure V.3: Feedback variables for the rear wheel based feedback control. θe is the difference between the tangent at the nearest point on the path to the rear wheel and the car heading. The magnitude of the scalar value e is illustrated in red. As illustrated e > 0, and for the case where the car is to the left of the path, e < 0.

s˙ e˙ ˙θe

where κ(s) denotes the curvature of the path at s. The following heading rate command provides local asymptotic convergence to twice continuously differentiable paths:

vf sin (δ + θe ) = −ke δ = arcsin(−ke/vf ) − θe .

(V.14)

The term θe in this case is not interpreted as heading error since it will be nonzero even with perfect tracking. It is more appropriately interpreted as a combination of a feed-forward term of the nominal steering angle to trace out the reference path and a heading error term. The drawback to this control law is that it is not defined when |ke/vf | > 1. The exponential convergence over a finite domain can be relaxed to local exponential convergence with the feedback law δ = arctan(−ke/vf ) − θe ,

(V.15)

which, to first order in e, is identical to the previous equation. This is illustrated in Figure V.4. Like the control law in (V.14) this controller locally exponentially stabilizes the car to paths with varying curvature with the condition that the path is continuously differentiable. The condition on the path arises from the definition of θe in the feedback policy. A drawback to this controller is that it is not stable in reverse making it unsuitable for parking.

20

Figure V.4: Front wheel output based control. The control strategy is to point the front wheel towards the path so that the component of the front wheel’s velocity normal to the path is proportional to the distance to the path. This is achieved locally and yields local exponential convergence.

Comparison of path tracking controllers for kinematic models: The advantages of controllers based on the kinematic model with the no-slip constraint on the wheels is that they have low computational requirements, are readily implemented, and have good performance at moderate speeds. Figure V.5 provides a qualitative comparison of the path stabilizing controllers of this sections based on, and simulated with, (III.3) for a lane change maneuver. In the simulation of the front wheel output based controller, the rear wheel reference path is replaced by the front wheel reference path satisfying xref (s) 7→ xref (s) + l cos(θ), (V.16) yref (s) 7→ yref (s) + l sin(θ). The parameters of the simulation are summarized in Table III. In reference to Figure V.5, the pure pursuit control tracks the reference path during periods with no curvature. In the region where the path has high curvature, the pure pursuit control causes the system to deviate from the reference path. In contrast, the latter two controllers converge to the path and track it through the high curvature regions. In both controllers using (V.5) in the feedback policy, local exponential stability can only be proven if there is a neighborhood of the path where (V.5) is continuous. Intuitively, this means the path cannot cross over itself and must be differentiable.

Figure V.5: Tracking performance comparison for the three path stabilizing control laws discussed in this section. (a) Pure pursuit deviates from reference when curvature is nonzero. (b) The rear wheel output based controller drives the rear wheel to the rear wheel reference path. Overshoot is a result of the second order response of the system. (c) The front wheel output based controller drives the front wheel to the reference path with a first order response and tracks the path through the maneuver. Example Parameters Wheel- Steering Initial Reference path base limit Configuration (xref (s), yref (s)) = (x (0), yr (0), θ(0)) L = 5 |δ| ≤ π/4 r (s, 4 · tanh s−40 = (0, −2, 0) 4 Controller Parameters Pure pursuit

Rear wheel feedback

Front wheel feedback

L = 5, vr = 1

ke = 0.25, kθ = 0.75, vr = 1

k = 0.5, vr = 1

Table III: Simulation and controller parameters used to generate Figure V.5.

B. Trajectory Tracking Control for the Kinematic Model 1) Control Lyapunov based design: A control design based on a control Lyapunov function is described in [142]. The approach is to define the tracking error in a coordinate frame fixed to the car. The configuration error can be expressed by a change of basis from the inertial coordinate frame using the reference trajectory, and velocity, (xref , yref , θref , vref , ωref ), xe cos(θ) sin(θ) 0 xref − xr ye = − sin(θ) cos(θ) 0 yref − yr . θe 0 0 1 θref − θ

The evolution of the configuration error is then x˙ e =ωye − vr + vref cos(θe ), y˙ e =−ωxe + vref sin(θe ), θ˙e =ωref − ω. With the control assignment, vr =vref cos(θe ) + k1 xe , ω =ωref + vref (k2 ye + k3 sin(θe )) .

21

the closed loop error dynamics become x˙ e = (ωref + vref (k2 ye + k3 sin(θe )))ye − k1 xe , y˙ e =− (ωref + vref (k2 ye + k3 sin(θe ))) xe + vref sin (θe ) , θ˙e =ωref − ω. Stability is verified for k1,2,3 > 0, ω˙ ref = 0, and v˙ ref = 0 by the Lyapunov function (1 − cos(θe )) 1 2 x + ye2 + , V = 2 e k2 with negative semi-definite time derivative, vref k3 sin2 (θe ) . V˙ = −k1 x2e − k2 A local analysis shows that the control law provides local exponential stability. However, for the system to be time invariant, ωref and vref are required to be constant. A related controller is proposed in [143] which utilizes a backstepping design to achieve uniform local exponential stability for a finite domain with time varying references. 2) Output feedback linearization: For higher vehicle speeds, it is appropriate to constrain the steering angle to have continuous motion as in (III.8). With the added state, it becomes more difficult to design a controller from simple geometric considerations. A good option in this case is to output-linearize the system. This is not easily accomplished using the front or rear wheel positions. An output which simplifies the feedback linearization is proposed in [144], where a point ahead of the vehicle by any distance d 6= 0, aligned with the steering angle is selected. Let xp = xf + d cos(θ + δ) and yp = yf + d sin(θ + δ) be the output of the system. Taking the derivative of these outputs and substituting the dynamics of III.8 yields

x˙ p = y˙ p vf cos(θ + δ) − dl sin(θ + δ) sin(δ) −d sin(θ + δ) . vδ sin(θ + δ) + dl cos(θ + δ) sin(δ) d cos(θ + δ) | {z }

To avoid confusion, note that in this case, the output position (xp , yp ) and controlled speed vf are not collocated as in the previously discussed controllers.

C. Predictive Control Approaches The simple control laws discussed above are suitable for moderate driving conditions. However, slippery roads or emergency maneuvers may require a more accurate model, such as the one introduced in Section III-B. The added detail of more sophisticated models complicates the control design making it difficult to construct controllers from intuition and geometry of the configuration space. Model predictive control [145] is a general control design methodology which can be very effective for this problem. Conceptually, the approach is to solve the motion planning problem over a short time horizon, take a short interval of the resulting open loop control, and apply it to the system. While executing, the motion planning problem is re-solved to find an appropriate control for the next time interval. Advances in computing hardware as well as mathematical programming algorithms have made predictive control feasible for real-time use in driverless vehicles. MPC is a major field of research on its own and this section is only intended to provide a brief description of the technique and to survey results on its application to driverless vehicle control. Since model predictive control is a very general control technique, the model takes the form of a general continuous time control system with control, u(t) ∈ Rm , and state, x(t) ∈ Rn , x˙ = f (x, u, t)

(V.21)

A feasible reference trajectory xref (t), and for some motion planners uref (t), are provided satisfying (V.21). The system is then discretized by an appropriate choice of numerical approximation so that (V.21) is given at discrete time instances by xk+1 =Fk (xk , uk ) ,

k ∈ N,

(V.22)

A(θ,δ)

(V.17) Then, defining the right hand side of (V.17) as auxiliary control variables ux and uy yields x˙ p ux = , (V.18) y˙ p uy which makes control straightforward. From ux and uy , the original controls vf and vδ are recovered by using the inverse of the matrix in (V.17), provided below: [A(θ, δ)]−1 = − d1

cos(θ + δ) sin(θ+δ)− 1l cos(θ+δ) sin(δ)

1 d

sin(θ + δ) cos(θ+δ)− 1l sin(θ+δ) sin(δ)

. (V.19)

From the input-output linear system, local trajectory stabilization can be accomplished with the controls ux = x˙ p,ref + kx (xp,ref − xp ), uy = y˙ p,ref + ky (yp,ref − yp ).

(V.20)

One of the simplest discretization schemes is Euler’s method with a zero order hold on control: Fk (x(k · ∆t), u(k · ∆t)) = , x(k · ∆t) + ∆t · f (x(k · ∆t), u(k · ∆t), tk )

k ∈ N.

(V.23) The state and control are discretized by their approximation at times tk = k · ∆t. Solutions to the discretized system are approximate and will not match the continuous time equation exactly. Similarly, the reference trajectory and control sampled at the discrete times tk will not satisfy the discrete time equation. For example, the mismatch between solutions to (V.23) and (V.21) will be O(∆t) and the reference trajectory sampled at time tk will result in Fk (xref (tk ), uref (tk )) − xref (tk+1 ) = O(∆t2 ).

(V.24)

To avoid over-complicating the following discussion, we assume the discretization is exact for the remainder of the

22

section. The control law typically takes the form uk (xmeas. ) = argmin h(xN − xref,N , uN − uref,N ) xn ∈ Xn , un ∈ Un +

k+N X−1

) gn (xn − xref,n , un − uref,n )

(V.25)

n=k

subject to xk = xmeas. xn+1 = F (xn , un ), n ∈ {k, ..., k + N − 1}. The function gn penalizes deviations from the reference trajectory and control at each time step, while the function h is a terminal penalty at the end of the time horizon. The set Xn is the set of allowable states which can restrict undesirable positions or velocities, e.g., excessive tire slip or obstacles. The set Un encodes limits on the magnitude of the input signals. Important considerations are whether the solutions to the right hand side of (V.25) exist, and when they do, the stability and robustness of the closed loop system. These issues are investigated in the predictive control literature [146], [147]. To implement an MPC on a driverless car, (V.25) must be solved several times per second which is a major obstacle to its use. In the special case that h and gn are quadratic, Un and Xn are polyhedral, and F is linear, the problem becomes a quadratic program. Unlike a general nonlinear programming formulation, interior point algorithms are available for solving quadratic programs in polynomial time. To leverage this, the complex vehicle model is often linearized to obtain an approximate linear model. Linearization approaches typically differ in the reference about which the linearization is computed— current operating point [148]–[150], reference path [151] or more generally, about a reference trajectory, which results in the following approximate linear model: xref,k+1 + ξk+1 = Fk (xref,k , uref,k ) +∇x Fk (xref,k , uref,k )ξk + ∇u Fk (xref,k , uref,k )ηk | {z } {z } | Ak

2

Bk

2

+O(kξk k ) + O(kηk k ), (V.26) where ξ := x − xref and η := u − uref are the deviations of the state and control from the reference trajectory. This first order expansion of the perturbation dynamics yields a linear time varying (LTV) system, ξk+1 = Ak ξk + Bk ηk .

(V.27)

Then using a quadratic objective, and expressing the polyhedral constraints algebraically, we obtain ( ) k+N X−1 T uk (xmeas. ) = argmin ξN HξN + ξkT Qk ξk +ηkT Rk ηk ξk ,ηk

n=k

subject to ξk = xmeas. − xref Cn ξn ≤ 0, Dn ηn ≤ 0 ξk+1 = Ak ξk + Bn ηn , n ∈ {k, ..., k + N − 1},

(V.28)

where Rk and Qk are positive semi-definite. If the states and inputs are unconstrained, i.e., Un = Rm , Xn = Rn , a semi-closed form solution can be obtained by dynamic programming requiring only the calculation of an N step matrix recursion [152]. Similar closed form recursive solutions have also been explored when vehicle models are represented by controlled auto-regressive integrated moving average (CARIMA) with no state and input constraints [153], [154]. A further variation in the model predictive control approach is to replace state constraints (e.g., obstacles) and input constraints by penalty functions in the performance functional. Such a predictive control approach based on a dynamic model with nonlinear tire behavior is presented in [155]. In addition to penalizing control effort and deviation from a reference path to a goal which does not consider obstacles, the performance functional penalizes input constraint violations and collisions with obstacles over the finite control horizon. In this sense it is similar to potential field based motion planning, but is demonstrated to have improved performance. The following are some variations of the model predictive control framework that are found in the literature of car controllers: 1) Unconstrained MPC with Kinematic Models: The earliest predictive controller in [153] falls under this category, in which the model predictive control framework is applied without input or state constraints using a CARIMA model. The resulting semi-closed form solution has minimal computational requirements and has also been adopted in [154]. Moreover, the time-varying linear quadratic programming approach with no input or state constraints was considered in [154] using a linearized kinematic model. 2) Path Tracking Controllers: In [151], a predictive control is investigated using a center of mass based linear dynamic model (assuming constant velocity) for path tracking and an approximate steering model. The resulting integrated model is validated with a detailed automatic steering model and a 27 degree-of-freedom CarSim vehicle model. 3) Trajectory Tracking Controllers: A predictive controller using a tire model similar to Section III was investigated in [148]. The full nonlinear predictive control strategy was carried out in simulation and shown to stabilize a simulated emergency maneuver in icy conditions with a control frequency of 20 Hz. However, with a control horizon of just two time steps the computation time was three times the sample time of the controller making experimental validation impossible. A linearization based approach was also investigated in [148]–[150] based on a single linearization about the state of the vehicle at the current time step. The reduced complexity of solving the quadratic program resulted in acceptable computation time, and successful experimental results are reported for driving in icy conditions at speeds up to 21m/s. The promising simulation and experimental results of this approach are improved upon in [150] by providing

23

conditions for the uniform local-asymptotic stability of the time varying system. D. Linear Parameter Varying Controllers Many controller design techniques are available for linear systems making a linear model desirable. However, the broad range of operating points encountered under normal driving conditions make it difficult to rely on a model linearized about a single operating point. To illustrate this, consider the lateral error dynamics in (V.10). If the tracking error is assumed to remain small a linearization of the dynamics around the operating point θe = 0 and e = 0 yields e˙ 0 vr e = θ˙e 0 0 θe (V.29) 0 0 + ω+ . 1 −vr κ(s) Introducing a new control variable incorporating a feedforward u = ω + vr κ(s) simplifies the discussion. The dynamics are now e˙ 0 vr e 0 = + u. (V.30) 0 0 θe 1 θ˙e Observe that the model is indeed linear, but the forward speed vr appears in the linear model. A simple proportional plus derivative control with gains kp and kd will stabilize the lateral dynamics but the polesof the closed loop system are given by p −kd ± kd2 − 4kp vr /2. At higher speeds the poles move into the complex plane leading to an oscillatory response. In contrast, a small kp gain leads to a poor response at low speed. A very intuitive and widely used remedy to this challenge is gain scheduling. In this example, parameterizing kp as a function of vr fixes the poles to a single value for each speed. This technique falls into the category of control design for linear parameter varying (LPV) models [156]. Gain scheduling is a classical approach to this type of controller design. Tools from robust control and convex optimization are readily applied to address more complex models. LPV control designs for lateral control are presented in [157]–[159]. LPV models are used in [160], [161] together with predictive control approaches for path and trajectory stabilization. At a lower level of automation, LPV control techniques have been proposed for integrated system control. In these designs, several subsystems are combined under a single controller to achieve improved handling performance. LPV control strategies for actuating active and semi-active suspension systems are developed in [162], [163], while integrated suspension and braking control systems are developed in [164], [165]. VI. C ONCLUSIONS The past three decades have seen increasingly rapid progress in driverless vehicle technology. In addition to the advances in computing and perception hardware, this rapid progress has been enabled by major theoretical progress in the computational aspects of mobile robot motion planning and feedback control theory. Research efforts have undoubtedly been spurred

by the improved utilization and safety of road networks that driverless vehicles would provide. Driverless vehicles are complex systems which have been decomposed into a hierarchy of decision making problems, where the solution of one problem is the input to the next. The breakdown into individual decision making problems has enabled the use of well developed methods and technologies from a variety of research areas. The task is then to integrate these methods so that their interactions are semantically valid, and the combined system is computationally efficient. A more efficient motion planning algorithm may only be compatible with a computationally intensive feedback controller such as model predictive control. Conversely, a simple control law may require less computation to execute, but is also less robust and requires using a more detailed model for motion planning. This paper has provided a survey of the various aspects of driverless vehicle decision making problems with a focus on motion planning and feedback control. The survey of performance and computational requirements of various motion planning and control techniques serves as a reference for assessing compatibility and computational tradeoffs between various choices for system level design. R EFERENCES [1] “2014 crash data key findings.” National Highway Traffic Safety Administration, Report No. DOT HS 812 219, 2014. [2] S. Singh, “Critical reasons for crashes investigated in the national motor vehicle crash causation survey.” National Highway Traffic Safety Administration, Report No. DOT HS 812 115, 2014. [3] B. McKenzie and M. Rapino, “Commuting in the United States: 2009, American Community Survey Reports,” U.S. Census Bureau, 2011. ACS-15, 2011. [4] D. A. Hennessy and D. L. Wiesenthal, “Traffic congestion, driver stress, and driver aggression,” Aggressive behavior, pp. 409–423, 1999. [5] E. D. Dickmanns and V. Graefe, “Dynamic monocular machine vision,” Machine vision and applications, vol. 1, pp. 223–240, 1988. [6] E. D. Dickmanns et al., “Vehicles capable of dynamic vision,” in IJCAI, pp. 1577–1592, 1997. [7] “No Hands Across America Journal.” http://www.cs.cmu.edu/afs/cs/ usr/tjochem/www/nhaa/Journal.html. Accessed: 2015-12-14. [8] M. Buehler, K. Iagnemma, and S. Singh, The 2005 DARPA Grand Challenge: The great robot race, vol. 36. Springer Science & Business Media, 2007. [9] M. Buehler, K. Iagnemma, and S. Singh, The DARPA Urban Challenge: Autonomous vehicles in city traffic, vol. 56. springer, 2009. [10] J. Xin, C. Wang, Z. Zhang, and N. Zheng, “China future challenge: Beyond the intelligent vehicle,” IEEE Intell. Transp. Syst. Soc. Newslett, vol. 16, pp. 8–10, 2014. [11] P. Cerri, G. Soprani, P. Zani, J. Choi, J. Lee, D. Kim, K. Yi, and A. Broggi, “Computer vision at the Hyundai autonomous challenge,” in International Conference on Intelligent Transportation Systems, pp. 777–783, IEEE, 2011. [12] A. Broggi, P. Cerri, M. Felisa, M. C. Laghi, L. Mazzei, and P. P. Porta, “The vislab intercontinental autonomous challenge: an extensive test for a platoon of intelligent vehicles,” International Journal of Vehicle Autonomous Systems, vol. 10, pp. 147–164, 2012. [13] A. Broogi, P. Cerri, S. Debattisti, M. C. Laghi, P. Medici, D. Molinari, M. Panciroli, and A. Prioletti, “Proud-public road urban driverlesscar test,” Transactions on Intelligent Transportation Systems, vol. 16, pp. 3508–3519, 2015. [14] J. Ziegler, P. Bender, M. Schreiber, H. Lategahn, T. Strauss, C. Stiller, T. Dang, U. Franke, N. Appenrodt, C. G. Keller, et al., “Making Bertha drive—an autonomous journey on a historic route,” Intelligent Transportation Systems Magazine, vol. 6, pp. 8–20, 2014. [15] “Google Self-Driving Car Project.” https://www.google.com/ selfdrivingcar/. Accessed: 2015-12-14. [16] “Tesla Motors: Model S Press Kit.” https://www.teslamotors.com/ presskit/autopilot. Accessed: 2016-3-15.

24

[17] S. O.-R. A. V. S. Committee et al., “Taxonomy and definitions for terms related to on-road motor vehicle automated driving systems,” 2014. [18] R. Rajamani, Vehicle dynamics and control. Springer Science & Business Media, 2011. [19] J. C. Gerdes and E. J. Rossetter, “A unified approach to driver assistance systems based on artificial potential fields,” Journal of Dynamic Systems, Measurement, and Control, vol. 123, pp. 431–438, 2001. [20] M. Brännström, E. Coelingh, and J. Sjöberg, “Model-based threat assessment for avoiding arbitrary vehicle collisions,” Transactions on Intelligent Transportation Systems, vol. 11, pp. 658–669, 2010. [21] A. Vahidi and A. Eskandarian, “Research advances in intelligent collision avoidance and adaptive cruise control,” Transactions on Intelligent Transportation Systems, vol. 4, pp. 143–153, 2003. [22] M. R. Hafner, D. Cunningham, L. Caminiti, and D. Del Vecchio, “Cooperative collision avoidance at intersections: Algorithms and experiments,” Transactions on Intelligent Transportation Systems, vol. 14, pp. 1162–1175, 2013. [23] A. Colombo and D. Del Vecchio, “Efficient algorithms for collision avoidance at intersections,” in Proceedings of the 15th ACM international conference on Hybrid Systems: Computation and Control, pp. 145–154, ACM, 2012. [24] H. Kowshik, D. Caveney, and P. Kumar, “Provable systemwide safety in intelligent intersections,” Vehicular Technology, IEEE Transactions on, vol. 60, pp. 804–818, 2011. [25] A. Eskandarian, Handbook of intelligent vehicles. Springer London, UK, 2012. [26] D. Miculescu and S. Karaman, “Polling-systems-based control of highperformance provably-safe autonomous intersections,” in Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on, pp. 1417– 1423, IEEE, 2014. [27] F. Zhou, X. Li, and J. Ma, “Parsimonious shooting heuristic for trajectory control of connected automated traffic part I: Theoretical analysis with generalized time geography,” arXiv preprint arXiv:1511.04810, 2015. [28] D. Geronimo, A. M. Lopez, A. D. Sappa, and T. Graf, “Survey of pedestrian detection for advanced driver assistance systems,” Transactions on Pattern Analysis & Machine Intelligence, pp. 1239–1258, 2009. [29] G. Ros, A. Sappa, D. Ponsa, and A. M. Lopez, “Visual SLAM for driverless cars: A brief survey,” in Intelligent Vehicles Symposium (IV) Workshops, 2012. [30] A. Geiger, J. Ziegler, and C. Stiller, “Stereoscan: Dense 3d reconstruction in real-time,” in Intelligent Vehicles Symposium (IV), 2011 IEEE, pp. 963–968, IEEE, 2011. [31] P. Liu, A. Kurt, K. Redmill, and U. Ozguner, “Classification of highway lane change behavior to detect dangerous cut-in maneuvers,” in The Transportation Research Board (TRB) 95th Annual Meeting, 2015. [32] E. W. Dijkstra, “A note on two problems in connexion with graphs,” Numerische mathematik, vol. 1, pp. 269–271, 1959. [33] N. J. Nilsson, “A mobile automaton: An application of artificial intelligence techniques,” tech. rep., DTIC Document, 1969. [34] A. V. Goldberg and C. Harrelson, “Computing the shortest path: A search meets graph theory,” in Proceedings of the sixteenth annual ACM-SIAM symposium on Discrete algorithms, pp. 156–165, Society for Industrial and Applied Mathematics, 2005. [35] R. Geisberger, P. Sanders, D. Schultes, and C. Vetter, “Exact routing in large road networks using contraction hierarchies,” Transportation Science, vol. 46, pp. 388–404, 2012. [36] H. Bast, D. Delling, A. Goldberg, M. Müller-Hannemann, T. Pajor, P. Sanders, D. Wagner, and R. F. Werneck, “Route planning in transportation networks,” arXiv preprint arXiv:1504.05140, 2015. [37] F. Havlak and M. Campbell, “Discrete and continuous, probabilistic anticipation for autonomous robots in urban environments,” Transactions on Robotics, vol. 30, pp. 461–474, 2014. [38] Q. Tran and J. Firl, “Modelling of traffic situations at urban intersections with probabilistic non-parametric regression,” in Intelligent Vehicles Symposium (IV), 2013 IEEE, pp. 334–339, IEEE, 2013. [39] A. C. Madrigal, “The trick that makes Google’s self-driving cars work,” The Atlantic, 2015. http://www.theatlantic.com/technology/ archive/2014/05/all-the-world-a-track-the-trick-that-makes-googlesself-driving-cars-work/370871/. [40] R. Verma and D. D. Vecchio, “Semiautonomous multivehicle safety,” Robotics & Automation Magazine, IEEE, vol. 18, pp. 44–54, 2011. [41] S. Z. Yong, M. Zhu, and E. Frazzoli, “Generalized innovation and inference algorithms for hidden mode switched linear stochastic systems

[42] [43]

[44]

[45]

[46] [47] [48] [49] [50] [51] [52] [53] [54]

[55] [56] [57]

[58] [59] [60] [61]

[62]

[63] [64] [65] [66] [67]

with unknown inputs,” in Decision and Control (CDC), 2014 IEEE 53rd Annual Conference on, pp. 3388–3394, IEEE, 2014. S. Brechtel, T. Gindele, and R. Dillmann, “Probabilistic MDP-behavior planning for cars,” in 14th International Conference on Intelligent Transportation Systems, pp. 1537–1542, IEEE, 2011. S. Ulbrich and M. Maurer, “Probabilistic online POMDP decision making for lane changes in fully automated driving,” in 16th International Conference on Intelligent Transportation Systems, pp. 2063– 2067, IEEE, 2013. S. Brechtel, T. Gindele, and R. Dillmann, “Probabilistic decisionmaking under uncertainty for autonomous driving using continuous POMDPs,” in 17th International Conference on Intelligent Transportation Systems, pp. 392–399, IEEE, 2014. E. Galceran, A. G. Cunningham, R. M. Eustice, and E. Olson, “Multipolicy decision-making for autonomous driving via changepoint-based behavior prediction,” in Proceedings of Robotics: Science & Systems Conference, p. 2, 2015. T. Bandyopadhyay, K. S. Won, E. Frazzoli, D. Hsu, W. S. Lee, and D. Rus, “Intention-aware motion planning,” in Algorithmic Foundations of Robotics X, pp. 475–491, Springer, 2013. S. M. LaValle, Planning algorithms. Cambridge university press, 2006. A. De Luca, G. Oriolo, and C. Samson, “Feedback control of a nonholonomic car-like robot,” in Robot motion planning and control, pp. 171–253, Springer, 1998. R. M. Murray and S. S. Sastry, “Nonholonomic motion planning: Steering using sinusoids,” Transactions on Automatic Control, vol. 38, pp. 700–716, 1993. T. Fraichard and R. Mermond, “Path planning with uncertainty for carlike robots,” in International Conference on Robotics and Automation, vol. 1, pp. 27–32, IEEE, 1998. M. Egerstedt, X. Hu, H. Rehbinder, and A. Stotsky, “Path planning and robust tracking for a car-like robot,” in Proceedings of the 5th symposium on intelligent robotic systems, pp. 237–243, Citeseer, 1997. R. C. Coulter, “Implementation of the pure pursuit path tracking algorithm,” tech. rep., DTIC Document, 1992. H. Goldstein, Classical mechanics. Pearson Education India, 1965. Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. P. How, and G. Fiore, “Real-time motion planning with applications to autonomous urban driving,” Transactions on Control Systems Technology, vol. 17, pp. 1105–1118, 2009. M. D. Ventures, “Stanley: The robot that won the DARPA Grand Challenge,” Journal of field Robotics, vol. 23, pp. 661–692, 2006. C. Samson, “Path following and time-varying feedback stabilization of a wheeled mobile robot,” in 2nd Int. Conf. on Automation, Robotics and Computer Vision, 1992. L. E. Dubins, “On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents,” American Journal of Mathematics, vol. 79, pp. 497– 516, 1957. J. Reeds and L. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific journal of mathematics, vol. 145, pp. 367–393, 1990. E. Velenis and P. Tsiotras, “Minimum time vs maximum exit velocity path optimization during cornering,” in International symposium on industrial electronics, pp. 355–360, 2005. A. Rucco, G. Notarstefano, and J. Hauser, “Computing minimum laptime trajectories for a single-track car with load transfer,” in Conference on Decision and Control, pp. 6321–6326, IEEE, 2012. J. H. Jeon, R. V. Cowlagi, S. C. Peters, S. Karaman, E. Frazzoli, P. Tsiotras, and K. Iagnemma, “Optimal motion planning with the halfcar dynamical model for autonomous high-speed driving,” in American Control Conference, pp. 188–193, IEEE, 2013. S. C. Peters, E. Frazzoli, and K. Iagnemma, “Differential flatness of a front-steered vehicle with tire force control,” in International Conference on Intelligent Robots and Systems (IROS), pp. 298–304, IEEE, 2011. E. Bakker, L. Nyborg, and H. B. Pacejka, “Tyre modelling for use in vehicle dynamics studies,” tech. rep., SAE Technical Paper, 1987. J. H. Reif, “Complexity of the mover’s problem and generalizations,” in 20th Annual Symposium on Foundations of Computer Science, SFCS ’79, (Washington, DC, USA), pp. 421–427, IEEE, 1979. J. Canny, The complexity of robot motion planning. MIT press, 1988. T. Lozano-Pérez and M. A. Wesley, “An algorithm for planning collision-free paths among polyhedral obstacles,” Communications of the ACM, vol. 22, pp. 560–570, 1979. J. A. Storer and J. H. Reif, “Shortest paths in the plane with polygonal obstacles,” Journal of the ACM, vol. 41, pp. 982–1012, 1994.

25

[68] M. H. Overmars and E. Welzl, “New methods for computing visibility graphs,” in Proceedings of the fourth annual symposium on Computational geometry, pp. 164–171, ACM, 1988. [69] M. De Berg, M. Van Kreveld, M. Overmars, and O. C. Schwarzkopf, Computational geometry. Springer, 2000. [70] S. Lazard, J. Reif, and H. Wang, “The complexity of the two dimensional curvatureconstrained shortest-path problem,” in Proceedings of the Third International Workshop on the Algorithmic Foundations of Robotics, pp. 49–57, 1998. [71] S. Fortune and G. Wilfong, “Planning constrained motion,” Annals of Mathematics and Artificial Intelligence, vol. 3, pp. 21–82, 1991. [72] P. K. Agarwal, T. Biedl, S. Lazard, S. Robbins, S. Suri, and S. Whitesides, “Curvature-constrained shortest paths in a convex polygon,” SIAM Journal on Computing, vol. 31, pp. 1814–1851, 2002. [73] J.-D. Boissonnat and S. Lazard, “A polynomial-time algorithm for computing a shortest path of bounded curvature amidst moderate obstacles,” in Proceedings of the twelfth annual symposium on Computational geometry, pp. 242–251, ACM, 1996. [74] F. Lamiraux, E. Ferre, and E. Vallee, “Kinodynamic motion planning: Connecting exploration trees using trajectory optimization methods,” in International Conference on Robotics and Automation, vol. 4, pp. 3987–3992, IEEE, 2004. [75] F. Boyer and F. Lamiraux, “Trajectory deformation applied to kinodynamic motion planning for a realistic car model,” in International Conference on Robotics and Automation, pp. 487–492, IEEE, 2006. [76] J. Canny and J. Reif, “New lower bound techniques for robot motion planning problems,” in 28th Symposium on Foundations of Computer Science, pp. 49–60, IEEE, 1987. [77] D. P. Bertsekas, Nonlinear programming. Athena scientific, 1999. [78] M. L. Fredman and R. E. Tarjan, “Fibonacci heaps and their uses in improved network optimization algorithms,” Journal of the ACM (JACM), vol. 34, pp. 596–615, 1987. [79] L. E. Kavraki, M. N. Kolountzakis, and J.-C. Latombe, “Analysis of probabilistic roadmaps for path planning,” Transactions on Robotics and Automation, vol. 14, pp. 166–171, 1998. [80] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, pp. 846–894, 2011. [81] E. Schmerling, L. Janson, and M. Pavone, “Optimal sampling-based motion planning under differential constraints: the driftless case,” International Conference on Robotics and Automation, 2015. [82] S. Karaman and E. Frazzoli, “Sampling-based optimal motion planning for non-holonomic dynamical systems,” in International Conference on Robotics and Automation, pp. 5041–5047, IEEE, 2013. [83] S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,” The International Journal of Robotics Research, vol. 20, pp. 378–400, 2001. [84] Y. Li, Z. Littlefield, and K. E. Bekris, “Sparse methods for efficient asymptotically optimal kinodynamic planning,” in Algorithmic Foundations of Robotics XI, pp. 263–282, Springer, 2015. [85] J. Reif and M. Sharir, “Motion planning in the presence of moving obstacles,” Journal of the ACM, vol. 41, pp. 764–790, 1994. [86] T. Fraichard, “Trajectory planning in a dynamic workspace: a’state-time space’approach,” Advanced Robotics, vol. 13, pp. 75–94, 1998. [87] J. Kasac, J. Deur, B. Novakovic, I. Kolmanovsky, and F. Assadian, “A conjugate gradient-based BPTT-like optimal control algorithm with vehicle dynamics control application,” Transactions on Control Systems Technology, vol. 19, pp. 1587–1595, 2011. [88] C. L. Darby, W. W. Hager, and A. V. Rao, “An hp-adaptive pseudospectral method for solving optimal control problems,” Optimal Control Applications and Methods, vol. 32, pp. 476–502, 2011. [89] L. S. Pontryagin, Mathematical theory of optimal processes. CRC Press, 1987. [90] Y. Tassa, N. Mansard, and E. Todorov, “Control-limited differential dynamic programming,” in International Conference on Robotics and Automation, pp. 1168–1175, IEEE, 2014. [91] J. T. Betts, “Survey of numerical methods for trajectory optimization,” Journal of Guidance, Control, and Dynamics, vol. 21, pp. 193–207, 1998. [92] E. Polak, “An historical survey of computational methods in optimal control,” SIAM Review, vol. 15, pp. 553–584, 1973. [93] B. Chazelle, “Approximation and decomposition of shapes,” Advances in Robotics, vol. 1, pp. 145–185, 1987. [94] C. Ó’Dúnlaing and C. K. Yap, “A "retraction" method for planning the motion of a disc,” Journal of Algorithms, vol. 6, pp. 104–111, 1985. [95] O. Takahashi and R. J. Schilling, “Motion planning in a plane using generalized voronoi diagrams,” Transactions on Robotics and Automation, vol. 5, pp. 143–150, 1989.

[96] J.-C. Latombe, Robot motion planning, vol. 124. Springer Science & Business Media, 2012. [97] J. T. Schwartz and M. Sharir, “On the "piano movers" problem. ii. general techniques for computing topological properties of real algebraic manifolds,” Advances in applied Mathematics, vol. 4, pp. 298–351, 1983. [98] J. Backer and D. Kirkpatrick, “Finding curvature-constrained paths that avoid polygonal obstacles,” in Proceedings of the twenty-third annual symposium on Computational geometry, pp. 66–73, ACM, 2007. [99] P. Jacobs and J. Canny, “Planning smooth paths for mobile robots,” in Nonholonomic Motion Planning, pp. 271–342, Springer, 1993. [100] H. Wang and P. K. Agarwal, “Approximation algorithms for curvatureconstrained shortest paths.,” in SODA, vol. 96, pp. 409–418, 1996. [101] S. M. La Valle, “Rapidly-exploring random trees a new tool for path planning,” tech. rep., Computer Science Dept., Iowa State University, 1998. [102] S. Petti and T. Fraichard, “Safe motion planning in dynamic environments,” in Intelligent Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on, pp. 2210–2215, IEEE, 2005. [103] A. Bhatia and E. Frazzoli, “Incremental search methods for reachability analysis of continuous and hybrid systems,” in Hybrid Systems: Computation and Control: 7th International Workshop, HSCC 2004, Philadelphia, PA, USA, March 25-27, 2004. Proceedings (R. Alur and G. J. Pappas, eds.), pp. 142–156, Springer Berlin Heidelberg, 2004. [104] E. Glassman and R. Tedrake, “A quadratic regulator-based heuristic for rapidly exploring state space,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on, pp. 5021–5028, IEEE, 2010. [105] S. Fleury, P. Soueres, J.-P. Laumond, and R. Chatila, “Primitives for smoothing mobile robot trajectories,” Transactions on Robotics and Automation, vol. 11, pp. 441–448, 1995. [106] E. Velenis, P. Tsiotras, and J. Lu, “Aggressive maneuvers on loose surfaces: Data analysis and input parametrization,” in Mediterranean Conference on Control Automation, pp. 1–6, 2007. [107] M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” Journal of Field Robotics, vol. 26, pp. 308–333, 2009. [108] S. M. LaValle, M. S. Branicky, and S. R. Lindemann, “On the relationship between classical grid search and probabilistic roadmaps,” The International Journal of Robotics Research, vol. 23, pp. 673–692, 2004. [109] J. Lengyel, M. Reichert, B. R. Donald, and D. P. Greenberg, Real-time robot motion planning using rasterizing computer graphics hardware, vol. 24. ACM, 1990. [110] L. E. Kavraki, P. Švestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” Transactions on Robotics and Automation, vol. 12, pp. 566– 580, 1996. [111] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions,” The International Journal of Robotics Research, 2015. [112] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic determination of minimum cost paths,” Transactions on Systems Science and Cybernetics, vol. 4, pp. 100–107, 1968. [113] I. Pohl, “First results on the effect of error in heuristic search,” Machine Intelligence, pp. 219–236, 1970. [114] A. Stentz, “Optimal and efficient path planning for partially-known environments,” in International Conference Robotics and Automation, pp. 3310–3317, IEEE, 1994. [115] S. Anthony, “The focussed D* algorithm for real-time replanning.,” in IJCAI, vol. 95, pp. 1652–1659, 1995. [116] S. Koenig and M. Likhachev, “Fast replanning for navigation in unknown terrain,” Transactions on Robotics, vol. 21, pp. 354–363, 2005. [117] E. A. Hansen and R. Zhou, “Anytime heuristic search.,” J. Artif. Intell. Res.(JAIR), vol. 28, pp. 267–297, 2007. [118] M. Likhachev, G. J. Gordon, and S. Thrun, “ARA*: Anytime A* with provable bounds on sub-optimality,” in Advances in Neural Information Processing Systems, p. None, 2003. [119] M. Likhachev, D. I. Ferguson, G. J. Gordon, A. Stentz, and S. Thrun, “Anytime dynamic A*: An anytime, replanning algorithm.,” in ICAPS, pp. 262–271, 2005. [120] K. Daniel, A. Nash, S. Koenig, and A. Felner, “Theta*: Any-angle path planning on grids,” Journal of Artificial Intelligence Research, pp. 533–579, 2010.

26

[121] A. Nash, S. Koenig, and C. Tovey, “Lazy theta*: Any-angle path planning and path length analysis in 3d,” in Third Annual Symposium on Combinatorial Search, 2010. [122] P. Yap, N. Burch, R. C. Holte, and J. Schaeffer, “Block a*: Databasedriven search with applications in any-angle path-planning.,” in AAAI, 2011. [123] D. Ferguson and A. Stentz, “Using interpolation to improve path planning: The field d* algorithm,” Journal of Field Robotics, vol. 23, pp. 79–101, 2006. [124] D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive configuration spaces,” in International Conference on Robotics and Automation, vol. 3, pp. 2719–2726, IEEE, 1997. [125] D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, “Randomized kinodynamic motion planning with moving obstacles,” The International Journal of Robotics Research, vol. 21, pp. 233–255, 2002. [126] T. Kunz and M. Stilman, “Kinodynamic RRTs with fixed time step and best-input extension are not probabilistically complete,” in Algorithmic Foundations of Robotics XI, pp. 233–244, Springer, 2015. [127] S. Karaman and E. Frazzoli, “Optimal kinodynamic motion planning using incremental sampling-based methods,” in Conference on Decision and Control, pp. 7681–7687, IEEE, 2010. [128] A. Perez, R. Platt Jr, G. Konidaris, L. Kaelbling, and T. LozanoPerez, “LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics,” in International Conference on Robotics and Automation, pp. 2537–2542, IEEE, 2012. [129] D. J. Webb and J. van den Berg, “Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics,” in International Conference on Robotics and Automation, pp. 5054–5061, IEEE, 2013. [130] M. Otte and E. Frazzoli, “RRT-X: Real-time motion planning/replanning for environments with unpredictable obstacles,” in International Workshop on the Algorithmic Foundations of Robotics, 2014. [131] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer, et al., “Autonomous driving in urban environments: Boss and the Urban Challenge,” Journal of Field Robotics, vol. 25, pp. 425–466, 2008. [132] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Practical search techniques in path planning for autonomous driving,” Ann Arbor, vol. 1001, p. 48105, 2008. [133] A. Bacha, C. Bauman, R. Faruque, M. Fleming, C. Terwelp, C. Reinholtz, D. Hong, A. Wicks, T. Alberi, D. Anderson, et al., “Odin: Team victorTango’s entry in the DARPA Urban Challenge,” Journal of Field Robotics, vol. 25, pp. 467–492, 2008. [134] J. Leonard, J. How, S. Teller, M. Berger, S. Campbell, G. Fiore, L. Fletcher, E. Frazzoli, A. Huang, S. Karaman, et al., “A perceptiondriven autonomous urban vehicle,” Journal of Field Robotics, vol. 25, pp. 727–774, 2008. [135] J. P. Hespanha et al., “Trajectory-tracking and path-following of underactuated autonomous vehicles with parametric modeling uncertainty,” Transactions on Automatic Control, vol. 52, pp. 1362–1379, 2007. [136] A. P. Aguiar, J. P. Hespanha, and P. V. Kokotovi´c, “Path-following for nonminimum phase systems removes performance limitations,” Transactions on Automatic Control, vol. 50, pp. 234–239, 2005. [137] H. K. Khalil, Nonlinear systems, vol. 3. Prentice hall New Jersey, 1996. [138] R. Wallace, A. Stentz, C. E. Thorpe, H. Maravec, W. Whittaker, and T. Kanade, “First results in robot road-following.,” in IJCAI, pp. 1089– 1095, 1985. [139] O. Amidi and C. E. Thorpe, “Integrated mobile robot control,” in Fibers’ 91, Boston, MA, pp. 504–523, International Society for Optics and Photonics, 1991. [140] A. L. Rankin, C. D. Crane III, D. G. Armstrong II, A. D. Nease, and H. E. Brown, “Autonomous path-planning navigation system for site characterization,” in Aerospace/Defense Sensing and Controls, pp. 176– 186, International Society for Optics and Photonics, 1996. [141] J. Wit, C. D. Crane, and D. Armstrong, “Autonomous ground vehicle path tracking,” Journal of Robotic Systems, vol. 21, pp. 439–449, 2004. [142] Y. Kanayama, Y. Kimura, F. Miyazaki, and T. Noguchi, “A stable tracking control method for an autonomous mobile robot,” in International Conference on Robotics and Automation, pp. 384–389, IEEE, 1990. [143] Z.-P. Jiang and H. Nijmeijer, “Tracking control of mobile robots: a case study in backstepping,” Automatica, vol. 33, pp. 1393–1399, 1997.

[144] B. d’Andréa Novel, G. Campion, and G. Bastin, “Control of nonholonomic wheeled mobile robots by state feedback linearization,” The International journal of robotics research, vol. 14, pp. 543–559, 1995. [145] C. E. Garcia, D. M. Prett, and M. Morari, “Model predictive control: theory and practice-a survey,” Automatica, vol. 25, pp. 335–348, 1989. [146] E. F. Camacho and C. B. Alba, Model predictive control. Springer Science & Business Media, 2013. [147] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, “Constrained model predictive control: Stability and optimality,” Automatica, vol. 36, pp. 789–814, 2000. [148] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat, “Predictive active steering control for autonomous vehicle systems,” Transactions on Control Systems Technology, vol. 15, pp. 566–580, 2007. [149] P. Falcone, M. Tufo, F. Borrelli, J. Asgari, and H. E. Tseng, “A linear time varying model predictive control approach to the integrated vehicle dynamics control problem in autonomous systems,” in 46th Conference on Decision and Control, pp. 2980–2985, IEEE, 2007. [150] P. Falcone, F. Borrelli, H. E. Tseng, J. Asgari, and D. Hrovat, “Linear time-varying model predictive control and its application to active steering systems: Stability analysis and experimental validation,” International journal of robust and nonlinear control, vol. 18, pp. 862– 875, 2008. [151] E. Kim, J. Kim, and M. Sunwoo, “Model predictive control strategy for smooth path tracking of autonomous vehicles with steering actuator dynamics,” International Journal of Automotive Technology, vol. 15, pp. 1155–1164, 2014. [152] D. E. Kirk, Optimal control theory: an introduction. Courier Corporation, 2012. [153] A. Ollero and O. Amidi, “Predictive path tracking of mobile robots. application to the CMU Navlab,” in 5th International Conference on Advanced Robotics, vol. 91, pp. 1081–1086, 1991. [154] G. V. Raffo, G. K. Gomes, J. E. Normey-Rico, C. R. Kelber, and L. B. Becker, “A predictive controller for autonomous vehicle path tracking,” Transactions on Intelligent Transportation Systems, vol. 10, pp. 92–102, 2009. [155] Y. Yoon, J. Shin, H. J. Kim, Y. Park, and S. Sastry, “Model-predictive active steering and obstacle avoidance for autonomous ground vehicles,” Control Engineering Practice, vol. 17, pp. 741–750, 2009. [156] O. Sename, P. Gaspar, and J. Bokor, Robust control and linear parameter varying approaches: application to vehicle dynamics, vol. 437. Springer, 2013. [157] P. Gáspár, Z. Szabó, and J. Bokor, “LPV design of fault-tolerant control for road vehicles,” International Journal of Applied Mathematics and Computer Science, vol. 22, pp. 173–182, 2012. [158] J. Huang and M. Tomizuka, “LTV controller design for vehicle lateral control under fault in rear sensors,” Transactions on Mechatronics, vol. 10, pp. 1–7, 2005. [159] S. Ç. Baslamisli, ˙I. E. Köse, and G. Anla¸s, “Gain-scheduled integrated active steering and differential control for vehicle handling improvement,” Vehicle System Dynamics, vol. 47, pp. 99–119, 2009. [160] T. Besselmann and M. Morari, “Autonomous vehicle steering using explicit LPV-MPC,” in Control Conference (ECC), 2009 European, pp. 2628–2633, IEEE, 2009. [161] T. Besselmann, P. Rostalski, and M. Morari, “Hybrid parameter-varying model predictive control for lateral vehicle stabilization,” in European Control Conference, pp. 1068–1075, IEEE, 2007. [162] P. Gáspár, Z. Szabó, and J. Bokor, “The design of an integrated control system in heavy vehicles based on an LPV method,” in Conference on Decision and Control, pp. 6722–6727, IEEE, 2005. [163] C. Poussot-Vassal, O. Sename, L. Dugard, P. Gaspar, Z. Szabo, and J. Bokor, “A new semi-active suspension control strategy through LPV technique,” Control Engineering Practice, vol. 16, pp. 1519–1534, 2008. [164] M. Doumiati, O. Sename, L. Dugard, J.-J. Martinez-Molina, P. Gaspar, and Z. Szabo, “Integrated vehicle dynamics control via coordination of active front steering and rear braking,” European Journal of Control, vol. 19, pp. 121–143, 2013. [165] P. Gaspar, Z. Szabo, J. Bokor, C. Poussot-Vassal, O. Sename, and L. Dugard, “Toward global chassis control by integrating the brake and suspension systems,” in 5th IFAC Symposium on Advances in Automotive Control, IFAC AAC 2007, p. 6, IFAC, 2007.

27

Brian Paden received B.S. and M.S. degrees in Mechanical Engineering in 2011 and 2013 respectively from UC Santa Barbara. Concurrently from 2011 to 2013 he was an Engineer at LaunchPoint Technologies developing electronically controlled automotive valve-train systems. His research interests are in the areas of control theory and motion planning with a focus on applications for autonomous vehicles. His current affiliation is with the Laboratory for Information and Decision systems and he is a Doctoral Candidate in the department of Mechanical Engineering at MIT. ˇ Michal Cáp received his Bc. degree in Information Technology from Brno University of Technology, the Czech Republic and MSc degree in Agent Technologies from Utrecht University, the Netherlands. He is currently pursuing PhD degree in Artificial Intelligence at CTU in Prague, Czech Republic. At the moment, Michal Cap holds the position of Fulbright-funded visiting researcher at Massachusetts Institute of Technology, USA. His research interests include motion planning, multi-robot trajectory coordination and autonomous transportation systems in general. Sze Zheng Yong is currently a postdoctoral associate in the Laboratory for Information and Decision Systems at Massachusetts Institute of Technology. He obtained his Dipl.-Ing.(FH) degree in automotive engineering with a specialization in mechatronics and control systems from the Esslingen University of Applied Sciences, Germany in 2008 and his S.M. and Ph.D. degrees in mechanical engineering from MIT in 2010 and 2016, respectively. His research interests lie in the broad area of control and estimation of hidden mode hybrid systems, with applications to intention-aware autonomous systems and resilient cyber-physical systems.

Dmitry Yershov was born in Kharkov, Ukraine, in 1983. He received his B.S. and M.S. degree in applied mathematics from Kharkov National University in 2004 and 2005, respectively. In 2013, he graduated with a Ph.D. degree from the the Department of Computer Science at the University of Illinois at Urbana-Champaign. He joined the Laboratory for Information and Decision Systems at the Massachusetts Institute of Technology in years 2013–2015 where he was a postdoctoral associate. Currently, he is with nuTonomy inc. His research is focused primarily on the feedback planning problem in robotics, which extends to planning in information spaces and planning under uncertainties. Emilio Frazzoli is a Professor of Aeronautics and Astronautics with the Laboratory for Information and Decision Systems at the Massachusetts Institute of Technology. He received a Laurea degree in Aerospace Engineering from the University of Rome, “Sapienza" , Italy, in 1994, and a Ph. D. degree from the Department of Aeronautics and Astronautics of the Massachusetts Institute of Technology, in 2001. Before returning to MIT in 2006, he held faculty positions at the University of Illinois, Urbana-Champaign, and at the University of California, Los Angeles. He was the recipient of a NSF CAREER award in 2002, and of the IEEE George S. Axelby award in 2015. His current research interests focus primarily on autonomous vehicles, mobile robotics, and transportation systems.