AutonoVi: Autonomous Vehicle Planning with Dynamic Maneuvers and Traffic Constraints
Abstract
We present AutonoVi:, a novel algorithm for autonomous vehicle navigation that supports dynamic maneuvers and satisfies traffic constraints and norms. Our approach is based on optimizationbased maneuver planning that supports dynamic lanechanges, swerving, and braking in all traffic scenarios and guides the vehicle to its goal position. We take into account various traffic constraints, including collision avoidance with other vehicles, pedestrians, and cyclists using control velocity obstacles. We use a datadriven approach to model the vehicle dynamics for control and collision avoidance. Furthermore, our trajectory computation algorithm takes into account traffic rules and behaviors, such as stopping at intersections and stoplights, based on an arcspline representation. We have evaluated our algorithm in a simulated environment and tested its interactive performance in urban and highway driving scenarios with tens of vehicles, pedestrians, and cyclists. These scenarios include jaywalking pedestrians, sudden stops from high speeds, safely passing cyclists, a vehicle suddenly swerving into the roadway, and highdensity traffic where the vehicle must change lanes to progress more effectively.
I Introduction
Autonomous driving is a difficult and extremely complex task that has immense potential for impacting the lives of billions of people. In order to develop autonomous capabilities to perform the driving task, we need appropriate capabilities to sense and predict the traffic and road obstacles, as well as for planning, control, and coordination of the vehicle [1, 2]. There is considerable research in this area that borrows ideas from different disciplines including computer vision, machine learning, motion planning, mechanical engineering, intelligent traffic simulation, humanfactors psychology, etc.
Research into sensing and perception technologies has been progressing considerably and current vehicle sensors seem to have the capability to detect relevant obstacles, vehicles, and other traffic entities including bicycles and pedestrians. However, automatic planning in different scenarios and the computation of the appropriate response to vehicle and nonvehicle entities, such as bicycles and pedestrians, are still the subjects of ongoing research. A key issue is the development of an efficient navigation algorithm for autonomous driving that takes into account the vehicle dynamics, sensor inputs, traffic rules and norms, and the driving behaviors of other vehicles. Moreover, the uncertainties in the sensor data, capability, and response of the autonomous vehicle, typically referred to as the egovehicle [3], have led to the development of behavior and navigation algorithms that impose conservative limits on the acceleration, deceleration, and steering decisions. For example, algorithms tend to limit hazard responses to either steering [4], [5] or braking [6]. Few algorithms demonstrate combined control of throttle and steering and typically do so in constrained navigation scenarios [7]. In terms of planning the routes and navigating the roads, current algorithms tend to limit the lanechanging behaviors, precluding their use for progressing more quickly to a goal or better utilization of the road conditions. These limitations have led to the perception that autonomous cars behave more like student drivers taking their driving test than actual skilled human drivers [3]. One of the goals is to extend the capabilities of current autonomous vehicles in terms of planning, control, and navigation, making them less conservative but still allowing safe performance during driving.
Main contributions: We present a novel navigation algorithm for autonomous vehicles, AutonoVi, which utilizes a datadriven vehicle dynamics model and optimizationbased maneuver planning to compute a safe, collisionfree trajectory with dynamic lanechanges. Our approach is general, makes no assumption about the traffic conditions, and plans dynamically feasible maneuvers in traffic consisting of other vehicles, cyclists, and pedestrians. In order to develop an autonomous vehicle planning approach with these capabilities, we present four novel algorithms:

Optimizationbased Maneuvering: We describe a novel multiobjective optimization approach for evaluating the dynamic maneuvers. Our algorithm encodes passenger comfort, safe passing distances, maneuver constraints in terms of dynamics, and global route progress in order to compute appropriate trajectories.

Data driven Vehicle Dynamics: We use a datadriven vehicle dynamics formulation that encodes feasible accelerations, steering rates, and decelerations into a set of pervehicle profile functions, which can be quickly evaluated. These profiles are generated by simulating the egovehicle through a series of trials to obtain lateral and longitudinal slip profiles. This datadriven model generalizes to multiple vehicles and configurations.

Collision avoidance with kinematic and dynamic constraints: We present a collision avoidance algorithm that combines collisionfree constraints with specific kinematic and dynamic constraints of the autonomous vehicle. Our approach allows the autonomous vehicle to steer away from collisions with other vehicles, pedestrians, and cyclists as well as to apply brakes, or use a combination of steering and braking.

Trajectory Planning with Traffic Rules and Behaviors: We present a trajectory planning algorithm that encodes traffic rules and road behaviors along with lanefollowing for computing safe trajectories. Our approach is based on computing arcs along the centerline of the current lane to generate an initial trajectory that satisfies all the constraints. This initial trajectory is computed and refined according to collision avoidance and maneuver optimization computations.
We evaluate our algorithm in a set of traffic scenarios generated using a physicsbased traffic simulator in both sparse and dense traffic conditions with tens of other vehicles, pedestrians, and cyclists. We demonstrate collisionavoidance events including a vehicle suddenly driving into the road, traffic suddenly stopping ahead of the egovehicle while travelling at high speed, and a pedestrian jaywalking in front of the egovehicle, representing typical accident scenarios [5]. Our approach enables advantageous of lane changes (e.g., overtaking) and adherence to traffic rules in typical traffic conditions. It also exhibits safe maneuvering in the presence of heavy traffic, pedestrians, and cyclists. To our knowledge, AutonoVi is the first approach that allows the egovehicle to follow an arbitrary route, determine appropriate lane changes dynamically during travel, and plan dynamically and kinematically feasible trajectories while following traffic norms and providing collision avoidance for vehicles, pedestrians, and cyclists.
The rest of the paper is organized as follows: we detail relevant related work in section 2. In section 3, we introduce the vehicle kinematic model, define relevant assumptions, and introduce the terminology used in the rest of the paper. In section 4, we present our navigation algorithm, AutonoVi, and its components. We present the details of our simulation benchmarks in section V and highlight the results in section VI.
Ii Related Work
The problem of autonomous driving has been widely studied in robotics, computer vision, intelligent transportation systems and related areas. In this section, we give a brief overview of prior methods which address motion planning and navigation, dynamics, behavior generation, and collision avoidance. More detailed surveys are given in [2, 8, 9].
Vehicle Kinematics and Dynamics Modeling: A number of approaches have been developed to model the motion of a moving vehicle. Different models offer a tradeoff between simplicity or efficiency of the approach, and physical accuracy. Simpler models are typically based on linear dynamics and analytical solutions to the equations of motion [10]. More accurate models provide a better representation of the physical motion, but require more computational power to evaluate and incorporate nonlinear forces in the vehicle dynamics [4]. The ReedsShepp formulation is a widely used car model with forward and backward gears [11]. Margolis and Asgari [12] present several representations of a car including the widely used singletrack bicycle model. Borrelli et al. [4] extend this model by including detailed tireforces. Current planning and control algorithms leverage varying levels of detail in the model of the vehicle.
Path Planning and Collision Avoidance: Prior approaches to path planning for autonomous vehicles are based on occupancy grids [13], randomexploration [14], driving corridors [15], potentialfield methods [16], etc. Recent approaches seek to incorporate driver behavior prediction in path planning using gametheoretic approaches [17] and Bayesian behavior modeling [18]. In addition, a variety of algorithms have been proposed for planning paths for automobiles for navigation outside of road conditions and traffic rules [19]. Several techniques have been proposed to specifically avoid hazards while remaining in a target lane. These techniques can be coupled with a path planner to avoid vehicles [20] and other hazards in the egovehicle’s lane [7].
Many continuous approaches for collisionavoidance have been proposed based on spatial decomposition or velocityspace reasoning. Berg et al. [21] apply velocityspace reasoning with acceleration constraints to generate safe and collisionfree velocities. Bareiss et al. [22] extend the concept of velocity obstacles into the control space to generate a complete set of collision free control inputs. Ziegler et al. [1] utilize polygonal decomposition of obstacles to generate blockages in continuous driving corridors. Sun et al. [23] demonstrate the use of prediction functions and trajectory set generation to plan safe lanechanges.
Modeling Traffic Rules: Aside from planning the appropriate paths to avoid collisions, autonomous vehicles must also follow applicable laws and traffic norms. Techniques have been proposed to simulate typical traffic behaviors in traffic simulation such as Human Driver Model [24] and datadriven models such as [25]. An extensive discussion on techniques to model these behaviors in traffic simulation can be found in [26].
Autonomous Driving Systems: Many autonomous systems have been demonstrated that are able to navigate an autonomous vehicle in traffic along a specific route. Ziegler et al. [3] demonstrated an autonomous vehicle which drove the historic Bertha Benz route in southern Germany. They use a conservative navigation approach, which specifically encodes lanelets for lane changing and does not account for dynamic lane changes. In contrast, our algorithm allows the vehicle to change lanes when our maneuver optimization method deems it appropriate and does not rely on preencoded changes. Geiger et al. [27] demonstrate a planning and control framework that won the Grand Cooperative Driving Challenge in 2011. This vehicle was designed for platooning and employed controls over acceleration only. Our navigation algorithm plans maneuvers using both steering and acceleration to operate in more generic traffic scenarios. The DARPA Urban Grand Challenge included a number of autonomous vehicle navigating examples of driving scenarios [28, 29]. While overtaking was allowed as an intended capability in these systems, the vehicles were not evaluated in dense, highspeed traffic conditions where the benefits of lane changes could be demonstrated.
Iii Problem Space
In this section, we introduce the notation, the kinematic and dynamics model of the car and the state space of the vehicle in terms of both physical configuration and behavior space.
Iiia Vehicle State Space
We represent the kinematic and dynamic constraints of the vehicle separately. In terms of trajectory planning, steering and throttle controls that could lead to skidding or a loss of control are first excluded in our dynamics model (see section IVF) and future trajectories are computed according to our vehicle kinematic model described in equation (1).
We extend the simplecar kinematic model [10, 30]. The vehicle has three degrees of freedom in a planar coordinate space. These are the position of the center of mass , and the current heading or orientation . We represent the speed of the vehicle as and steering as . and represent the distance from the center of mass to the front and rear axles, respectively. The geometry of the egovehicle is represented as .
The vehicle has two degrees of control, throttle () and steering (). We define throttle , where indicates maximum braking effort for the vehicle and represents maximum throttle. describes the steering effort from to .
We also use acceleration and steering functions, and , respectively, which describe the relationship between the vehicle’s speed, steering, and control inputs and its potential for changes in the acceleration and steering (see section IVF). and can be chosen to be constants in the simplest model, or may be represented using complex functions corresponding to tire dynamics and load transfer. We describe our choice for and in section IVF. The vehicle’s motion can be described by:
(1a)  
(1b) 
In addition to the physical state of the vehicle, we describe its behavior as a label from a set of all behaviors , such as driving straight, turning left, merging right, etc. The behavior state is used to modify parameters of each stage of the algorithm. Each behavior state can encode a set of weights of the maneuver optimization function, bias the generation of a guiding path, and adjust the sampling bias of our controlobstacle approximation and acceleration when necessary (see section IVA). The full state of a vehicle is defined as . The vehicle updates its plan at a fixed planning rate . At each planning step, the vehicle computes a target speed and target steering to be achieved by the control system. We refer to equation (1) compactly as the state evolution function . We also define a function which determines if a set of controls is feasible. Given the current state of the vehicle, will return false if the given input will cause a loss of traction or control. We describe this function further in section IVF.
IiiB Sensing and Perception
We assume a sensing module is available for the vehicle that is capable of providing information regarding the surrounding environment. For each lane on a road, the sensing module provides an approximation of the center line of the lane, . The sensing module also provides the closest point on the lane center to the egovehicle, , and a reasonable value of the friction coefficient . Recent work has presented approaches to evaluate from sensor data [31]. Our navigation algorithm utilizes the set of nearby vehicles, pedestrians, bicycles, or other obstacles, collectively referred to as neighbors, within the sensing range. For each neighbor , the sensing system provides the neighbor’s shape, , position, , and velocity . Moreover, the sensing module provides the lane , acceleration , and rate of turn, for the neighbor. We define a set of neighbor types, , including vehicle, pedestrian, cyclist, and obstruction. Each neighbor is assigned a type corresponding to the detected neighbor type. The complete state of a neighbor is denoted as
Iv Navigation Algorithm
In this section, we describe our navigation algorithm. Our algorithmic approach operates in four sequential stages, shown in Fig. 1. First, a route is constructed over the space of roads in the environment. Secondly, a Guiding Path that follows the current lane is computed that provides input to the collisionavoidance and optimizationbased maneuver stages. The collision avoidance stage determines the set of feasible candidate controls that represent dynamically feasible, collisionfree controls for the vehicle. Finally, a new control is chosen for the vehicle based on the optimizationbased maneuver function.
Iva Route Choice and Behavior State
Our navigation algorithm performs several steps in a sequential manner. In the first step, a global route for the vehicle to follow to the goal is determined. This step is performed only once unless special conditions (e.g., missing a turn) force the vehicle to recompute a route. The egovehicle is provided a connected graph of roads in the environment from a GIS database. Each road in the graph contains information on the number and configuration of lanes in the road and the speed limits. When a destination is chosen, we use A* search to compute the shortest route to the goal and construct a route plan.
Each step of the route plan encodes how the vehicle transitions from one road to the next. We denote these as roadtransition maneuvers. A roadtransition maneuver consists of the valid source lanes, valid destination lanes, the position along the road at which the maneuver begins, denoted , and the behavior implied by the road transition. The set of behaviors includes merging, right turns, left turns, and driving straight. Once the roadchange maneuver is completed, the vehicle navigates along the lanes of the new road until the next maneuver node is reached. Lane changes are not encoded in the maneuver nodes, but they are performed implicitly based on the optimization function described in section IVE.
The behavior state of the vehicle is described by a finitestate machine shown in figure 2. It is used to restrict potential control decisions and adjust the weight of the costfunction for specific maneuvers, such as turning. This allows our algorithm to force the vehicle to be more conservative when performing delicate maneuvers. For example, the valid steering space is constrained in turns to guarantee that the vehicle moves closely along the center line.
IvB Guiding Path
In order to perform trajectory planning, the egovehicle computes a set of waypoints along the centerline of its current lane at fixed time intervals, and these waypoints represent the expected positions for a planning horizon, . We represent these waypoints as . Using its own position, the median point, and the final waypoint (), we compute a circular arc on the road plane which sets the initial target speed and steering, and respectively, and acts as the guiding path for the next planning phase. We use circular arc approximations because they implicitly encode the radius of curvature needed for slip computation making it easy to check whether the dynamic constraints are violated.
Absent discontinuities in the centerline of the lane, the guiding arcs exhibit firstorder continuity. Figure 3(a) demonstrates a guiding arc constructed for a sample lane.
We constrain the arcs to lie within the first two quadrants of the circle that is represented by three waypoints. In cases when the vehicle’s trajectory tracks away from the center of the lane, e.g. during collision avoidance maneuvers, this constraint may be violated as shown in figure 3(b). In such cases, the point is reflected about the axis formed between and to correct the arc angle. In case of lane changing, waypoints are constructed from a weighted average of points sampled ahead on both the departure lane and the destination lane. Figure 3(c) demonstrates a set of lanechange arcs.
Given a guiding path, a target steering, is computed from equation (1a). The radius of the arc, , is substituted into equation (8) to determine the maximum safe speed for the current road curvature. A target speed, , is computed from the minimum value of the current speed limit and the maximum safe speed. The target steering and speed form the basis of the controlobstacle exploration in the subsequent stage.
IvB1 Traffic Rules
Traffic rules such as stopping at red lights are encoded in our algorithm. When choosing a target speed , the sensing system is referenced to determine if an intersection is being approached and whether the vehicle needs to stop at the intersection. In cases where the vehicle must stop, the edge of the intersection is used to compute a stopping point and is set to the speed that will reach the stopping point at seconds. In case of stoplights, the green light signals to return to its original value.
In the case of stops with continuous crosstraffic, the vehicle waits until the collisionavoidance algorithm indicates safety. This is accomplished by limiting the potential speed controls the vehicle may choose. When waiting for crosstraffic, the vehicle will stop until its guiding path is determined to be safe. In the case of allway stops, the vehicle maintains a queue of vehicle arrival order, but defers to other drivers if they enter the intersection out of turn.
Although merges are not specifically encoded in transitions in the route plan, the vehicle is able to determine when merging is safe through the collisionavoidance and optimization stages of the algorithm. A merge is note determined safe and appropriate unless it provides collisionfree guarantees and respects safety and comfort costs detailed in section IVE.
IvC Collision Avoidance
We leverage the theory of Control Obstacles for collision avoidance [22]. Control Obstacles construct constraints in the control space and are an extension of accelerationvelocity obstacles [21]. For each neighbor of the egovehicle, , we define the control obstacle for the neighbor as the union of all controls that could lead to collisions with the neighbor within the time horizon, . Given t, where , the relative position of the egovehicle and neighbor must remain outside the Minkowski Sum given by the formulation, which is defined as
(2) 
The complete derivation for control obstacles can be found in [22].
In order to adapt to the autonomous vehicles, we modify the original control obstacle formulation [22] in the following manner: (1) We do not assume reciprocity in collision avoidance and the egovehicle must take full responsibility for avoiding collisions; (2) We do not assume the control inputs of other vehicles are observable, which is consistent with the first point; (3) We do not assume bounding discs for the neighboring entities, but rather a tight bounding rectangle. The Minkowski Sum for two convex polygons can be computed in linear time in the number of edges; (4) The new feasible control chosen does not correspond to the control that minimizes the deviation from and . Rather it is the control that minimizes the objective function defined in section IVE.
The union of all controlobstacles and the set of dynamically infeasible controls form the boundary of the space of collisionfree controls for the egovehicle. As long as a new control set is chosen from outside the union of the control obstacles, the egovehicle will be collision free for the next seconds. In section VII, we detail how behavior prediction models can be incorporated to assume varying levels of reciprocity. This approach is conservative and it is possible that there may be no feasible solution. In that case, we reduce and search for a feasible solution.
IvD Trajectory Sampling
Computing the exact boundary of the control obstacle is computationally expensive. Moreover, depending on the choice of and , the boundary computation will typically not have an analytical solution. In order to ensure that the vehicle can plan within a specific time bound, we use a sampling strategy around and to determine a feasible control that the vehicle will adopt for the next seconds. Each sample is referred to as a candidate control and represented as .
First, the closest collisionfree velocity to is determined where by forward projection. This represents the largest speed the vehicle could take without deviating from the centerline of its lane and is always included in the set of candidates. Next, we compute evenly spaced samples around the point in the control space. We also choose a set of samples around the prior step solution, and , which allows the vehicle to explore minor deviations in trajectory. Samples near the prior solution facilitate lanekeeping and withinlane avoidance maneuvers.
For each neighbor , we compute a set of states for that neighbor for the next seconds by forward integration of . We assume that the neighboring vehicle will follow its current lane at the current speed and acceleration during this time interval. Otherwise, the neighbor is assumed to move along its current velocity with the current observed values of turning and acceleration, and , respectively.
For each candidate control, , we determine whether equation (8) is violated by the candidate control inputs and immediately discard it if that is the case. If not, the sample points are computed at even time intervals along by forward integration of . For each position in time, , we compute the relative position with each neighboring position at that time and determine if the relative position lies inside the Minkowski Sum. If so, we discard the candidate controls. After all the candidates are evaluated, the new control sequence is chosen by minimizing the objective function described in the subsequent section.
IvE New Trajectory Computation
Once a set of suitable control candidates has been computed, the vehicle selects the valid controls that minimize the following cost function at each sample point :
(3) 
This function corresponds to producing paths which are comfortable for passengers, provide safe passingdistances from other vehicles, and respect the constraints of upcoming maneuvers the vehicle must perform. Each term consists of several cost evaluation functions, each with its own weight , which are described in the following sections.
IvE1 Path Cost
encodes costs associated with the vehicle’s success at tracking its path and the global route. To compute this cost, we define two points. The target point, , represents the relative position the vehicle would achieve following the spline defined by its guiding path exactly at . Given the final sample, we project the vehicle’s expected position at the final sample point onto , which we denote . The path cost is given by:
(4)  
is the squared difference between desired speed and current speed and is the squared distance between the center line of the vehicle’s lane and its current position. If the path crosses a lane boundary, is computed with respect to the new lane. represents the vehicle’s desire to maximally progress along its current path. Candidates which reduce progress with respect to the guiding path are penalized. These terms drive the vehicle to choose trajectories that maximally progress the egovehicle along its computed route between steps. is only computed at the final sample point.
IvE2 Comfort Costs
Comfort costs are computed similar to [1] and penalize motions which are uncomfortable for passengers in the vehicle. penalizes large accelerations and decelerations. penalizes large heading changes and discourages sharp turning. The comfort costs are given as:
(5)  
IvE3 Maneuver Costs
The novel maneuvering cost function discourages lanechanges without excluding them and guides the vehicle to occupy the necessary lane for its next maneuver. The formulation is given as:
(6)  
is a boolean variable representing whether a candidate path crosses a lane boundary. is the position of the next maneuver change, e.g. the beginning of a right turn. This position is determined by the point of maneuver and starts in the desired lane for the maneuver. is a boolean that evaluates to true if the vehicle’s lane does not match the lane for the next maneuver. If a candidate control is chosen where for some point , evaluates to true, a lane change behavior is initiated in the finite state machine.
IvE4 Proximity Costs
While the collision avoidance stage prevents the vehicle from colliding with neighbors, the proximity cost term is designed to prevent the vehicle from passing close to neighboring entities based on the identified type of the neighbor, . This cost is represented as a cost distance term with exponential decay based on the relative positions of the egovehicle and its neighbor.
(7)  
is a pertype constant cost value. is larger for pedestrians and bicycles than for vehicles, and guides the egovehicle to pass those entities with greater distance.
IvF Datadriven Vehicle Dynamics Model
In order to determine values for and , we use a datadriven approach to model the dynamics of the vehicle. For each egovehicle, data is collected by driving the vehicle from to at the highest possible throttle without loss of traction. Similarly, for braking, the vehicle is decelerated from to using the highest braking effort possible without loss of traction. Data is collected at Hz for these values: current speed, acceleration, and throttle/braking values. From these data, piecewise quadratic functions are constructed by least squares fitting to represent the maximum available acceleration and braking given the current vehicle state. These values also define thresholds for the control safety function .
We determine by fixing the vehicle’s speed and collecting data for instantaneous changes to the steering angle for a given . We construct a piecewise quadratic function by leastsquares fitting to represent the vehicle’s steering dynamics. Having the value of from the sensors, we determine the maximum feasible speed for a given curvature from the centripetal force equation:
(8) 
where is the radius of curvature that is computed from equation (1a). By substituting equation (1a) into equation (8), and the angular velocity formula , we can determine feasible steering for a given speed as
(9) 
Given the generated functions , , and , the future path of the vehicle can be evaluated quickly for planning future controls.
IvG Control Input
Once a new set of controls is chosen, they are input to the vehicle using a pair of PID controllers. One of the PID controllers drives the current speed to match the target speed. The second PID controller drives the current steering angle to match the target steering angle chosen by the optimization function. By limiting the choice of candidate controls to kinematically and dynamically feasible controls using our datadriven vehicle dynamics model, the PID controllers are sufficient to achieve the desired values.
V Experimental Evaluation
In this section, we detail the evaluation scenarios for our navigation algorithm. Each scenario is chosen to test different aspects of the algorithm including response time, safety, and handling different traffic situations.
Va EgoVehicles
To demonstrate the generality of our approach, we tested each experimental scenario on each of three vehicles. Vehicle 1, the hatchback, has a mass of kg, a length of m, and a maximum steering angle of . Vehicle 2, the sports car, has a mass of kg, a length of m, and a maximum steering angle of . Vehicle 3, the SUV, has a mass of kg, a length of m, and a maximum steering angle of .
VB Benchmarks
We conducted a series of simulations with each vehicle representing a variety of the challenging traffic scenarios an egovehicle will face while navigating city roads and highways.
Passing a bicycle: This scenario involves the egovehicle passing a bicycle on a four lane straight road. The vehicle should maintain a safe distance from the bicycle, changing lanes if possible to avoid the cyclist. We perform the evaluation twice, once featuring a vehicle in the adjacent lane preventing the vehicle from moving to avoid the cyclist without first adjusting its speed.
Jaywalking Pedestrian: This scenario features a pedestrian stepping into the road in front of the vehicle. The vehicle must react quickly to safely decelerate or stop to avoid the pedestrian.
Sudden Stop at High Speed: The vehicle must execute an emergency stop on a highway at high speeds when the vehicle in front of it stops suddenly. We evaluate this scenario in two conditions. First, we evaluate performance with no other traffic aside from the egovehicle and stopping vehicle. In this condition, swerving can be performed simply. Secondly, we evaluate this scenario with surrounding traffic, complicating any swerving maneuvers as the vehicle must account for nearby traffic.
High Density Traffic Approaching a Turn: This scenario features a four lane road with the egovehicle starting in a heavily congested outer lane. The egovehicle must make a turn at a stoplight ahead in the outer lane. To make optimal progress, the egovehicle must execute a lane change to the inner lane, but must return to the outer lane with sufficient time to execute the turn.
Car Suddenly entering Roadway: This scenario demonstrates the egovehicle traveling along a straight road at constant speed when a vehicle suddenly enters the roadway ahead of the vehicle and blocks the vehicle’s path. The vehicle must decelerate and swerve to avoid colliding with the blocking vehicle. We demonstrate this scenario with the egovehicle travelling at 10, 30, and 50 mph and with the blocking vehicle obstructing either the right lane or both lanes.
Sturns: We demonstrate the egovehicle navigating a set of tight alternating turns, or S turns. Each egovehicle computes a different safe speed depending on the specific kinematic and dynamic limits of the vehicle.
Simulated City: We demonstrate the egovehicle navigating to several key points in a small simulated city. The vehicle must execute lane changes to perform various turns as it obeys traffic laws and navigates to its goals. The vehicle encounters bicycles, pedestrians, and other vehicles as it navigates to its waypoints.
Vi Benchmark results
We evaluated our navigation algorithm in these simulated scenarios. The algorithm can avoid tens of vehicles at interactive rates. As expected, the sportscar and the hatchback were able to maintain their preferred speeds more effectively in turns, whereas our SUV was forced to reduce speed. Each of the vehicles was able to pass other vehicles, pedestrians, and bicycles safely. In Car Suddenly entering Roadway scenario, we observed a greater tendency to swerve. We did not observe the egovehicle colliding with any of the simulated vehicles in traffic.
Figure 4 details some of the interesting behaviors we observed while testing our navigation algorithm. As expected, the egovehicle utilizes lanechanges to pass slower vehicles when no traffic is imposing. In traffic, the egovehicle slows down until it is safe to pass in the adjoining lane. When interacting with pedestrians, the high proximity cost discourages the vehicle from changing lanes as the pedestrian passes, and the vehicle instead waits until the pedestrian has moved considerably.
Via Timing Results
We collected data from a simulation designed to gradually increase the density of other vehicles encountered by our car. Figure 5 demonstrates the relationship between collision avoidance cost for a specific trajectory sample and the number of nearby vehicles and entities. We observe the cost of collision avoidance grows linearly in the number of neighbors.
Figure 6 demonstrates the relationship between the number of trajectories sampled and the computational expense of optimization evaluation. The observed relationship is linear with greater variance than that of collision avoidance computation. The overall computation time for a typical navigation update including guiding path computation, controlobstacle sampling, collisionavoidance and cost evaluation is on the order of milliseconds, typically between 1 and 2 milliseconds. This suggests that the cost of the algorithm is dominated by the optimization time.
Vii Conclusion and Limitations
We present, AutonoVi, a navigation algorithm for autonomous vehicles. Our approach uses a datadriven vehicle dynamics model and optimizationbased maneuver planning to compute safe, collision free trajectories with dynamic lane changes under typical traffic conditions. We have demonstrated our algorithm on a varied set of vehicles under varying dense and sparse traffic conditions with pedestrians and cyclists. We have also demonstrated that our vehicles follow traffic laws, and utilize both braking and steering simultaneously when avoiding collisions. We highlight many benefits over prior methods in our simulations.
Our approach has some limitations. First, though our introduction of the datadriven dynamics functions , , and generalize to arbitrary levels of underlying dynamics complexity, our current approach requires computing new vehicle dynamics functions for different values of . We will address this limitation in future work by learning a transfer function between various road frictions to produce more general datadriven vehicle dynamics functions. In addition, we have assumed perfect sensing in the current technique and it would be useful to take into account sensing errors and uncertainty in our approach. These could be based on relying on predictive behavior models to overcome imperfect state estimations for neighboring entities [18, 23]. With prediction, our control obstacles could anticipate levels of reciprocity from predictable vehicles. We will also explore whether the use of circular arcs may is appropriate for vehicles with substantially different geometries, such as trucks pulling large trailers. It is unclear if our kinematic models and datadriven profiles will navigate large vehicles safely. We will also like to incorporate realworld driving patterns and cultural norms to improve our navigation algorithm. This will include choosing optimal weights for the navigation algorithm using available training data.
References
 [1] Julius Ziegler, Philipp Bender, Thao Dang, and Christoph Stiller. Trajectory planning for Bertha  A local, continuous method. The International Journal of Robotics Research, 35(April):450–457, 2014.
 [2] Pendleton et al. Perception, Planning, Control, and Coordination for Autonomous Vehicles. Machines, 5(1):6, 2017.
 [3] Ziegler et al. Making bertha drivean autonomous journey on a historic route. IEEE Intelligent Transportation Systems Magazine, 6(2):8–20, 2014.
 [4] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat. MPCbased approach to active steering for autonomous vehicle systems. International Journal of Vehicle Autonomous Systems, 3(2/3/4):265, 2005.
 [5] Andreas Eidehall, Jochen Pohl, Fredrik Gustafsson, and Jonas Ekmark. Toward autonomous collision avoidance by steering. IEEE Transactions on Intelligent Transportation Systems, 8(1):84–94, 2007.
 [6] Martin Distner, Mattias Bengtsson, Thomas Broberg, and Lotta Jakobsson. City Safety A System Addressing RearEnd Collisions At Low Speeds. 21st Enhanced Safety Vehicles Conference, pages 1–7, 2009.
 [7] Turri et al. Linear model predictive control for lane keeping and obstacle avoidance on low curvature roads. IEEE Conference on Intelligent Transportation Systems, Proceedings, ITSC, (Itsc):378–383, 2013.
 [8] Christos Katrakazas, Mohammed Quddus, WenHua Chen, and Lipika Deka. Realtime motion planning methods for autonomous onroad driving: Stateoftheart and future research directions. Transportation Research Part C: Emerging Technologies, 60:416–442, 2015.
 [9] Mohammad Saifuzzaman and Zuduo Zheng. Incorporating humanfactors in carfollowing models: A review of recent developments and research needs. Transportation Research Part C: Emerging Technologies, 48:379–403, 2014.
 [10] Steven M. LaValle. Planning Algorithms. Cambridge University Press, New York, NY, USA, 2006.
 [11] James Reeds and Lawrence Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific Journal of Mathematics, 145(2):367–393, 1990.
 [12] Donald L. Margolis and Jahan Asgari. Multipurpose models of vehicle dynamics for controller design. In SAE Technical Paper. SAE International, 09 1991.
 [13] Sascha Kolski, D. Ferguson, Mario Bellino, and Roland Siegwart. Autonomous Driving in Structured and Unstructured Environments. In 2006 IEEE Intelligent Vehicles Symposium, pages 558–563. IEEE, 2006.
 [14] Kuwata et al. Realtime motion planning with applications to autonomous urban driving. IEEE Transactions on Control Systems Technology, 17(5):1105–1118, 2009.
 [15] Jason Hardy and Mark Campbell. Contingency Planning Over Probabilistic Obstacle Predictions for Autonomous Road Vehicles. IEEE Transactions on Robotics, 29(4):913–929, aug 2013.
 [16] Enric Galceran, Ryan M Eustice, and Edwin Olson. Toward Integrated Motion Planning and Control using Potential Fields and Torquebased Steering Actuation for Autonomous Driving. IEEE Intelligent Vehicles Symposium, (Iv), 2015.
 [17] Dorsa Sadigh, Shankar Sastry, Sanjit A Seshia, and Anca D Dragan. Planning for Autonomous Cars that Leverage Effects on Human Actions. Proceedings of Robotics: Science and Systems, 2016.
 [18] Enric Galceran, Alexander G Cunningham, Ryan M Eustice, and Edwin Olson. Multipolicy DecisionMaking for Autonomous Driving via Changepointbased Behavior Prediction. Robotics: Science and Systems, 2015.
 [19] Julius Ziegler, Moritz Werling, and Joachim Schröder. Navigating carlike robots in unstructured environments using an obstacle sensitive cost function. IEEE Intelligent Vehicles Symposium, Proceedings, pages 787–791, 2008.
 [20] H. Fritz, A. Gern, H. Schiemenz, and C. Bonnet. CHAUFFEUR Assistant: a driver assistance system for commercial vehicles based on fusion of advanced ACC and lane keeping. IEEE Intelligent Vehicles Symposium, 2004, (Vc):495–500, 2004.
 [21] Jur Van Den Berg, Jamie Snape, Stephen J. Guy, and Dinesh Manocha. Reciprocal collision avoidance with accelerationvelocity obstacles. Proceedings  IEEE International Conference on Robotics and Automation, pages 3475–3482, 2011.
 [22] Daman Bareiss and Jur van den Berg. Generalized reciprocal collision avoidance. The International Journal of Robotics Research, 34(12):1501–1514, oct 2015.
 [23] Hao Sun, Weiwen Deng, Sumin Zhang, Shanshan Wang, and Yutan Zhang. Trajectory planning for vehicle autonomous driving with uncertainties. ICCSS 2014  Proceedings: 2014 International Conference on Informative and Cybernetics for Computational Social Systems, pages 34–38, 2014.
 [24] Martin Treiber, Arne Kesting, and Dirk Helbing. Delays, inaccuracies and anticipation in microscopic traffic models. Physica A: Statistical Mechanics and its Applications, 360(1):71–88, 2006.
 [25] Peter Hidas. Modelling vehicle interactions in microscopic simulation of merging and weaving. Transportation Research Part C: Emerging Technologies, 13(1):37–62, 2005.
 [26] Bo Chen and Harry H. Cheng. A review of the applications of agent technology in traffic and transportation systems. IEEE Transactions on Intelligent Transportation Systems, 11(2):485–497, 2010.
 [27] Geiger et al. Team AnnieWAY’s Entry to the 2011 Grand Cooperative Driving Challenge. IEEE Transactions on Intelligent Transportation Systems, 13(3):1008–1017, 2012.
 [28] Chris Urmson and et al. Autonomous Driving in Traffic: Boss and the Urban Challenge. Al Magazine, 30(2):17–28, 2009.
 [29] Bacha et al. Odin: Team VictorTango’s entry in the DARPA Urban Challenge. Journal of Field Robotics, 25(8):467–492, aug 2008.
 [30] Jean paul Laumond, S. Sekhavat, and F. Lamiraux. Guidelines in nonholonomic motion planning for mobile robots. In ROBOT MOTION PLANNNING AND CONTROL, pages 1–53. SpringerVerlag, 1998.
 [31] Fredrik Gustafsson. Slipbased tireroad friction estimation. Automatica, 33(6):1087–1099, 1997.