ALGAMES: A Fast Solver for Constrained Dynamic Games

Simon Le Cleac’h1, Mac Schwager2, and Zachary Manchester2

Abstract— Dynamic games are an effective paradigm fordealing with the control of multiple interacting actors. Currentalgorithms for solving these problems either rely on Hamilton-Jacobi-Isaacs (HJI) methods, dynamic programming (DP),differential dynamic programming (DDP), or an iterative bestresponse approach (IBR). The first two approaches have strongtheoretical guarantees; however they becomes intractable inhigh-dimensional real-world applications. The third approachis grounded in the success of iLQR. It is scalable, but it cannothandle constraints. Finally, the iterative best response algorithmis a heuristic approach with unknown convergence properties,and it can suffer from stability and tractability issues. Thispaper introduces ALGAMES (Augmented Lagrangian GAME-theoretic Solver), a solver that handles trajectory optimizationproblems with multiple actors and general nonlinear state andinput constraints. We evaluate our solver in the context ofautonomous driving on scenarios involving numerous vehiclessuch as ramp merging, overtaking, and lane changing. Wepresent simulation and timing results demonstrating the speedand the ability of the solver to produce efficient, safe, andnatural autonomous behaviors.

I. INTRODUCTION

Controlling a robot in an environment where it interactswith other actors is a complex task. Traditional approaches inthe literature adopt a partitioned approach. First, predictionsof other actors’ trajectories are computed, then they are fedinto a trajectory optimizer that considers them as immutableobstacles. This approach is limiting because the effect of therobot’s trajectory on the other actors is ignored. Moreover,it can lead to the “freezing robot” problem that arises whenthe planner finds that all paths to the goal are unsafe [1].The consequence is that the robot stops moving or executesunnecessary anti-collision maneuvers.

For this reason, dealing with the game-theoretic aspect ofthe planning problem is a critical issue that has a broad rangeof applications. For instance, in autonomous driving, rampmerging, lane changing, intersection crossing, and overtakingmaneuvers comprise some degree of game-theoretic interac-tions [2], [3], [4], [5], [6], [7]. Other potential applicationsinclude mobile robots navigating in crowds, like packagedelivery robots, tour guides, or domestic robots. Also, robotsinteracting with people in factories such as mobile robots, orfixed-base multi-link manipulators. Finally, it is applicablein competitive settings, e.g. drone and car racing [8], [9].

In this work, we propose to solve constrained multi-player general-sum dynamic games. In dynamic games, theplayers’ strategies are sequences of decisions. It is important

1Department of Mechanical Engineering, Stanford University, Stanford,USA simonlc@stanford.edu

2Department of Aeronautics & Astronautics, Stanford University, Stan-ford, USA {schwager,zacmanchester}@stanford.edu

Fig. 1. Sequence of images depicting the trajectories obtained by solvingfor Nash equilibrium strategies. The images appear in chronological orderfrom left to right. The vehicles achieve a ramp merging and a lane changewhile avoiding collision with other vehicles and respecting road boundaries.

to notice that unlike traditional optimization problem, non-cooperative games have no “optimal” solution. Dependingon the structure of the game, asymmetry between players,etc., one achieves different concepts of solutions. In thiswork, we search for Nash equilibrium solutions. This typeof equilibrium models symmetry between the players; allplayers are treated equally. At such equilibria, no player canreduce his cost by unilaterally changing his strategy. Forextensive details about the game-theory concepts addressedin this paper, we refer readers to the work of Bressan [10]and Basar et al. [11].

Our solver is aimed at finding a Nash equilibrium formulti-player dynamic games, and can handle general nonlin-ear state and input constraints. This is particularly importantfor robotic applications, where the agents often interactthrough their desire to avoid collisions with one anotheror with the environment. Such interaction is most natu-rally, and most correctly, represented as (typically nonlinear)state constraints. This is a crucial point that sets game-theoretic methods for robotics apart from game-theoreticmethods in other domains, such as economics, behavioralsciences, and robust control. In these domains, the agentinteractions are traditionally represented in the objectivefunctions themselves, and these games typically have nostate or input constraints. In mathematical game theory, Nashequilibria with constraints are referred to as GeneralizedNash Equilibria [12]. Hence, in this paper we present anaugmented Lagrangian solver for finding Generalized NashEquilibria specifically tailored to robotics applications.

Our solver assumes that players are rational agents actingto minimize their costs. This rational behavior is formulatedusing the first-order necessary condition for Nash equilibria,analogous to the Karush-Kuhn-Tucker (KKT) conditions

in optimization. By relying on an augmented Lagrangianapproach to robustly handle constraints, the solver is able tosolve multi-player games with numerous agents and a highlevel of interactions at speeds approaching real-time. Ourprimary contributions are:

1) A general solver for dynamic games aimed at identi-fying Generalized Nash Equilibrium strategies.

2) Demonstration of the solver’s speed, robustness, andscalability in autonomous driving applications (Fig. 1).

II. RELATED WORK

A. Equilibrium Selection

Recent work focused on solving multi-player dynamicgames can be categorized by the type of equilibrium theyselect. Several works [2], [3], [9], [13] opted for the searchof Stackelberg equilibria, which models an asymmetry of in-formation between players. It is usually formulated for gameswith two players, a leader and a follower. The leader chooseshis strategy first, then the follower selects the best responseto the leader’s strategy. Alternatively, a Nash equilibriumdoes not introduce hierarchy between players; each player’sstrategy is the best response to the other players’ strategies.As pointed out in [6], searching for open-loop Stackelbergequilibrium strategies can fall flat on simple examples. In thecontext of autonomous driving, for instance, when players’cost functions only depend on their own state and controltrajectories, the solution becomes trivial. The leader canignore mutual collision constraints and the follower has toadapt to this strategy. This behavior can be overly aggressivefor the leader (or overly passive for the follower) and doesnot capture the game-theoretic nature of the problem.

Search for Nash equilibria has been investigated in [4], [5],[8], [14]. We also take the approach of searching for Nashequilibria, as this type of equilibrium seems better suitedto symmetric, multi-robot interaction scenarios. Indeed, wehave observed more natural behavior emerging from Nashequilibria compared to Stackelberg when solving for open-loop strategies.

B. Game-Theoretic Trajectory Optimization

Most of the algorithms proposed in the robotics literatureto solve for game-theoretic equilibria can be grouped intothree types. First, algorithms relying on a decompositionmethod such as Jacobi or Gauss-Siedel methods [8], [14],[15]. These algorithms are based on an iterative best responsescheme. All the players take turns at improving their strate-gies, considering the other agents’ strategies as immutable. Itis aimed at finding Nash equilibria. This type of approach iseasy to interpret and handles games with numerous playerswell. However, convergence of these algorithms is not wellunderstood [12], and special care is required to capture thegame-theoretic nature of the problem [8]. Moreover, solvingfor a Nash equilibrium until convergence can require manyiterations, each of which is a (possibly expensive) trajectoryoptimization problem. This can lead to prohibitively longsolution times.

Second, there are a variety of algorithms based on dynamicprogramming. In [6], a Markovian Stackelberg strategy iscomputed via dynamic programming. This approach seemsto capture the game-theoretic nature of autonomous driving.However, dynamic programming suffers from the curse ofdimensionality, and therefore relies on a simplified dynamicsmodel coupled with a coarse discretization of the stateand input space. To counterbalance these approximations, alower-level planner informed by the state values under theMarkovian Stackelberg strategy is run. This approach, whichscales exponentially with the state dimension, has only beendemonstrated in a two-player setting. Adding more playerswould prevent real-time application of this algorithm. Ourproposed approach, on the contrary, scales reasonably withthe number of players (see Fig. 4).

Finally, algorithms akin to differential dynamic program-ming have been developed for robust control [16] and laterapplied to game-theoretic problems [4]. This approach scalespolynomially with the number of players and is potentiallyfast enough to run real-time in a model predictive control(MPC) fashion. However, this approach does not handle con-straints. Collision-avoidance constraints are typically han-dled using large penalties that can result in numerical ill-conditioning and a brittle solver. Moreover, it leads to a trade-off between trajectory efficiency and avoiding collisionswith other players. This approach seems questionable in theautonomous driving context. Our approach, however, canenforce nonlinear state and input constraints in a rigorousway.

C. Generalized Nash Equilibrium Problems

As mentioned above, we focus on finding Nash equilibriafor multi-player games in which players are coupled throughshared state constraints (such as collision-avoidance con-straints). Therefore, these problems are instances of Gener-alized Nash Equilibrium Problems (GNEPs). The operationsresearch field has a rich literature on GNEPs [17], [18], [19],[20], [21]. Exact penalty methods have been proposed tosolve GNEPs [18], [19]. Complex constraints such as thosethat couple players’ strategies are handled using penalties.This allows solution of multi-player games jointly for allthe players, while still being able to reason about complexconstraints. However, these exact penalty methods requireminimization of nonsmooth objective functions, which turnsout to be slow in practice. In the same vein, a penaltyapproach relying on an augmented Lagrangian formulationof the problem has been advanced by Pang et al. [17]. Thiswork, however, converts the augmented Lagrangian formula-tion to a set of KKT conditions, including complementarityconstraints. The resulting constraint-satisfaction problem issolved with an off-the-shelf linear complementarity problem(LCP) solver that exploits the linearity of a specific problem.Our solver, on the contrary, is not tailored for a specificexample and can solve general GNEPs. It draws inspirationfrom this augmented Lagrangian formulation, which does notintroduce nonsmooth terms in the objective function so thesolution can be found quickly. Moreover, this formulation

avoids ill-conditioning, which makes our solver numericallyrobust.

III. PROBLEM STATEMENT

Following the formalism of Facchinei [12], we considerthe GNEP with M players. Each player ν controls thevariables pν ∈Rnp . We denote by ppp the concatenated vectorof the individual decision variables,

ppp =[(p1)T . . . (pM)T

]T, (1)

with dimension q = Mnp. By ppp−ν , we denote the vector ofall the players’ decision variables except those of player ν .The cost function of each player is noted Jν : Rq → R. Itdepends on player ν’s variables pν as well as on all the otherplayers’ variables ppp−ν . The goal of player ν is to select astrategy pν that minimizes his cost function Jν , given theother players’ strategies ppp−ν . In addition, the strategy pν

must belong to a set Pν(ppp−ν), and we express this constraintwith a concatenated set of inequality constraints Cν : Rq→Rnc . Formally,

minpν

Jν(pν , ppp−ν),

s.t. Cν(pν , ppp−ν)≤ 0.(2)

A solution of the GNEP (a generalized Nash equilibrium),is a vector p̄pp such that for all ν = 1, . . . ,M, p̄ν is a solutionto (2) with the other players’ strategies fixed to p̄pp−ν . Thismeans that at an equilibrium point p̄pp, no player can decreasehis cost by unilaterally changing his strategy pν to any otherfeasible point.

In the discretized trajectory optimization setting with Ntime steps, we denote by n the state size, m the con-trol input size, xν

t the state, and uνt the control input of

player ν at the time step t. In this context, the decisionvariables of each player pν designate the primal vari-ables associated with this player. They are the sequencesof states Xν = [(xν

1 )T . . .(xν

N)T ]T and control inputs Uν =

[(uν1 )

T . . .(uνN−1)

T ]T of player ν , i.e.

pν =[(Xν)T (Uν)T ]T . (3)

Thus, when solving for a generalized Nash equilibrium of thegame ppp, we identify open-loop Nash equilibrium trajectories,in the sense that the control signal is a function of time,not of the state variables of the players. However, one canrepeatedly resolve the open-loop game as new informationis obtained over time to obtain a policy that is closed-loop in the model-predictive control sense. The cost functionJν encodes the objective of player ν . The concatenatedset of constraints Cν includes dynamics constraints and,in the context of autonomous driving, collision constraintscoupled between players. This formulation is general enoughto comprise multi-player general-sum dynamic games withnonlinear constraint on the states and control inputs.

IV. AUGMENTED LAGRANGIAN FORMULATION

We propose an algorithm to solve the previously definedGNEP in the context of trajectory optimization. We expressthe fact that players are acting optimally to minimize theircost functions under constraints as an equality. To do so,we first derive the augmented Lagrangian associated with(2) solved by each player. Then, we use the fact that, at anoptimal point, the gradient of the augmented Lagrangian isnull [22]. Therefore, at a generalized Nash equilibrium point,the gradients of the augmented Lagrangians of all playersmust be null. This is a set of M equality constraints that wesolve using a quasi-Newton root-finding algorithm.

A. Individual Optimality

First, without loss of generality, we suppose that the vectorCν is actually the concatenated set of inequality and equalityconstraints, i.e. Cν = [Cν

iT Cν

eT ]T ∈Rnci+nce , where Cν

i ≤ 0 isthe vector of inequality constraints and Cν

e = 0 is the vectorof equality constraints. To embed the notion that each playeris acting optimally, we formulate the augmented Lagrangianassociated with (2) for player ν . We denote by λ ν ∈Rnc theLagrange multipliers associated with the vector of constraintsCν ; ρν ∈ Rnc is a penalty weight vector.

Lν(pν , ppp−ν) = Jν +λν TCν +

12

Cν T Iρν Cν . (4)

Where Iρν is a diagonal matrix defined as,

Iρν ,kk =

{0 if Cν

k (pν , ppp−ν)< 0 ∧ λ νk = 0, k ≤ nci,

ρνk otherwise,

(5)

where k = 1, . . . ,nci +nce indicates the kth constraint. Giventhe appropriate Lagrange multipliers λ ν , the gradient of theaugmented Lagrangian with respect to the individual primalvariables ∇pν Lν = Gν is null at an optimal point of (2). Thefact that player ν is acting optimally to minimize Jν underthe constraints Cν can therefore be expressed as follows,

∇pν Lν(pν , ppp−ν) = Gν(pν , ppp−ν) = 0. (6)

It is important to note that this equality constraint preservescoupling between players since the gradient Gν depends onthe other players’ strategies ppp−ν .

B. Root-Finding Problem

At a generalized Nash equilibrium, all players are actingoptimally. Therefore, to find an equilibrium point, we haveto solve the following root-finding problem,

minppp

0,

s.t. Gν(pν , ppp−ν)= 0, ∀ ν ∈ {1, . . . ,M}.(7)

We use Newton’s method to solve the root-findingproblem. We denote by G the concatenation of theaugmented Lagrangian gradients of all players G(ppp) =[(G1)T , . . . ,(GM)T ]T . We compute the first order derivative ofG with respect to all primal variables, H = ∇pppG. Newton’s

method allows us to identify a search direction δ ppp in theprimal variables space,

δ ppp =−H−1G. (8)

We couple this search direction with a backtracking line-search [23] detailed in Algorithm 1 to ensure local conver-gence to a solution using Newton’s Method [23] presentedin Algorithm 2.

Algorithm 1 Backtracking line-search1: procedure LINESEARCH(ppp,G,δ ppp)2: Parameters3: α = 1,4: β ∈ (0,1/2),5: τ ∈ (0,1),6: Until ||G(ppp+αδ ppp)||2 < (1−αβ )||G(ppp)||2 do7: α ← τα

8: return α

Algorithm 2 Newton’s method for root-finding problem1: procedure NEWTON’SMETHOD(ppp)2: Until Convergence do3: G← [(∇p1 L1)T , . . . ,(∇pM LM)T ]T

4: H← ∇pppG5: δ ppp←−H−1G6: α ← LINESEARCH(ppp,G,δ ppp)7: ppp← ppp+αδ ppp8: return ppp

Algorithm 3 ALGAMES solver1: procedure ALGAMES(ppp0,ρ0)2: Initialization3: λ ν ← 0, ∀ν4: ρν ← ρ0, ∀ν5: ppp← ppp06: Until Convergence do7: ppp← NEWTON’SMETHOD(ppp)8: λ ν ← DUALASCENT(ppp,λ ν ,ρν), ∀ν9: ρν ← INCREASINGSCHEDULE(ρν), ∀ν

10: return ppp

C. Augmented Lagrangian Updates

To obtain convergence of the Lagrange multipliers λ ν , weupdate them with a dual-ascent step. This update can be seenas shifting the value of the penalty terms into the Lagrangemultiplier terms,

λνk ←

{max(0,λ ν

k +ρνk Cν

k (ppp)) k ≤ nci,

λ νk +ρν

k Cνk (ppp) nci < k ≤ nci +nce.

(9)

We also update the penalty weights according to an increas-ing schedule, with γ > 1:

ρνk ← γρ

νk , ∀k ∈ {1, . . . ,nc}. (10)

D. ALGAMES

By combining Newton’s method for finding the pointwhere gradients of the augmented Lagrangians are null withthe Lagrange multiplier and penalty updates, we obtain oursolver ALGAMES (Augmented Lagrangian GAME-theoreticSolver) presented in Algorithm 3. The algorithm, whichiteratively solves the GNEP, requires as inputs an initial guessfor the primal variables ppp0 and initial penalty weights ρ0. Thealgorithm outputs the primal variables ppp containing the open-loop strategies of all players. Finding a Nash equilibriumis a non-convex problem in general. There is, therefore, noguarantee about convergence to the global optimum, and ouralgorithm requires a reasonable initial guess to converge.

V. SIMULATIONS: DESIGN AND SETUP

We choose to apply our algorithm in the autonomousdriving context. Indeed, many maneuvers like lane changing,ramp merging, overtaking, and intersection crossing involvea high level of interaction between vehicles. Our game-theoretic planner could improve performance in these in-teractive tasks compared to traditional planners that do notconsider coupled interactions among all the vehicles. Weassume a single car is computing the trajectories for allcars in its neighborhood, so as to find its own trajectoryto act safely among the group. In a real application, thiscomputation would be repeated as frequently as possible inan MPC fashion.

A. Autonomous Driving Problem

1) Constraints: Each vehicle in the scene is an actor of thegame. Our objective is to find a generalized Nash equilibriumtrajectory for each vehicle. These trajectories have to bedynamically feasible. The dynamics constraints at time stept are expressed as follows,

xνt+1 = f (xν

t ,uνt ). (11)

Although the solver is able to deal with nonlinear constraintsarising from complex dynamics models, we consider onlydouble-integrator dynamics. A vehicle state xν

t is composedof a 2D position and a 2D velocity. The control input uν

t is the2D acceleration. The dynamics constraints can be expressedas,

xνt+1−Axν

t −Buνt = 0. (12)

In addition, it is critical that the trajectories respect collision-avoidance constraints. We model the collision zone of thevehicles as circles of radius r. The collision constraintsbetween vehicles are then simply expressed in terms of theposition x̃ν

t of each vehicle,

r2−||x̃νt − x̃µ

t ||22 ≤ 0, ∀ ν ,µ ∈ {1, . . . ,M},ν 6= µ. (13)

We also model boundaries of the road to force the vehicles toremain on the roadway. This means that the distance betweenthe vehicle and the closest point, q, on each boundary, b, hasto remain larger than the collision circle radius, r,

r2−||x̃νt −qb||22 ≤ 0, ∀ b, ∀ ν ∈ {1, . . . ,M}. (14)

Finally, we enforce a final state constraint on a subset ofthe state dimensions. With this constraint we can enforce,for instance, a final velocity or a final position of thevehicle along a particular direction. In summary, based onreasonable simplifying assumptions, we have expressed thedriving problem in terms of both linear individual constraintsand non-convex coupled constraints.

2) Cost Function: We use a quadratic cost function penal-izing the use of control inputs and the distance between thecurrent state and the desired final state xν

f of the trajectory,

Jν(pν) =N

∑t=1

12(xν

t − xνf )

T Q(xνt − xν

f )+12

uνk

T Ruνk . (15)

This cost function only depends on the decision variables pν

of vehicle ν . Players’ behaviors are coupled only throughcollision constraints. We could also add terms depending onother vehicles’ strategies, such as a congestion penalty.

B. Driving Scenarios

We test our solver on three different driving scenariosinvolving strong interactions between vehicles:

1) Ramp Merging: First, we set up a roadway with hardboundaries as pictured in Fig. 1 to demonstrate a ramp-merging maneuver. We position multiple vehicles on theroadway in a collision-free initial configuration. We choosea reasonable desired final state where the incoming vehiclehas merged into the traffic. We purposefully place numerousplayers in a relatively confined space to maximize the levelof interaction between players. Our objective is to generategeneralized Nash equilibrium trajectories for all the vehicles.These trajectories are collision-free and cannot be improvedunilaterally by any player.

2) Lane Changing: The objective for each vehicle is tochange lanes while avoiding collisions (Fig. 6). This situationis challenging because it involves a high level of negotiation[7] between drivers in a real-world setting, which results instrongly coupled trajectories.

3) Overtaking: A fast vehicle is placed behind a slowerone (Fig. 7). The faster vehicle performs an overtakingmaneuver to maintain its desired speed.

VI. SIMULATIONS: RESULTS

A. Robustness, Speed, and Scalability

1) Robustness: To get a better understanding of the algo-rithm, we plot the L1-norm of the concatenated gradients ofthe individual Lagrangians ||G||1 and the condition numberof the second order derivative matrix H in Fig. 2. It corre-sponds to the ramp merging experiment presented in Figure5. The gradient curve, similar to a sawtooth wave, surgesin value due to dual-ascent updates that impact the value ofthe Lagrangians. The root-finding algorithm gets the gradientto converge towards zero in few iterations. We observe thatthe condition number of H incrementally increases aftereach penalty update. However, it remains in a reasonablerange during the solve. This reasonable conditioning of thenumerical problem, combined with the consistent behavior

0 20 40 60 8010−17

10−3

1011

Iterations

||G||1Condition(H)

Fig. 2. Behavior of the L1-norm of the gradient G and the conditionnumber of the matrix H during the solve.

0 20 40 60 80

10−4

10−1

102

Iterations

||Car col.||∞||Bound col.||∞||Dynamics||∞

Fig. 3. Convergence of the constraint satisfaction during the solve forthe 3-vehicle ramp merging example. The plotted constraints are collisionavoidance between vehicles, collision avoidance with road boundaries, anddynamic feasibility. The end of the solve is triggered when all the constraintsare satisfied up to a threshold ε = 10−2.

of the root-finding method, exhibits the robustness of thesolver. We also plot constraint satisfaction in Fig. 3, wherewe observe a linear convergence of the maximum constraintviolations.

2) Scalability: Scalability of the algorithm to scenarioswith more than two players is highly desirable. Indeed,driving problems like lane merging often involve 3 or 4players. We solved for five-second trajectories (N = 25time steps) while increasing number of actors from 2 to8 on one core of an AMD Ryzen 2950x processor. Fig. 4demonstrates near-real-time performance on scenarios with 2and 3 players (2.5s and 6.1s respectively). The solving timeincreases reasonably with the number of players. Comparedto other approaches that scale exponentially with the numberof players [6], our method is still tractable for up to 8 players(one-minute solve time for a 5s trajectory).

B. Results From Driving Scenarios

1) Ramp Merging: As pictured in Fig. 5, we observethat the merging vehicle in blue is squeezing between theother two vehicles. They are adapting their trajectories to letthe blue vehicle merge smoothly. We also see that the bluevehicle is taking a trajectory that accommodates the othervehicles by squeezing against the ramp boundary representedby a black line on Fig. 5. The algorithm demonstratesnear real-time performance as it takes 6.1s to solve the 5strajectory on one core of an AMD Ryzen 2950x processor.

We then test the solver on a more complex problem with5 vehicles. In addition to the 3 ramp-merging vehicles, weadd 2 vehicles and one of them is performing a lane change.Fig. 1 presents the highly coupled trajectories that we obtain.We observe that the red and green vehicles are slightlynudging on their right to accommodate for the yellow vehicle

2 4 6 80

204060

Number of players M

Tim

e(s)

IterationsTime

6080100120

Iter

atio

ns

Fig. 4. Solving time and number of iterations required to reach theconstraint satisfaction threshold vs. the number of players. All scenariosfrom 2 to 8 players are of comparable complexity. They include one rampmerging and multiple lane changes when the number of players is sufficient.

Fig. 5. Ramp merging scenario: the blue vehicle successfully merges onthe highway by squeezing between the other two vehicles.

overtaking them. This example demonstrates the robustnessof the solver to complex multi-player interactions as well asits scalability. This 5s trajectory is solved in 25.1s.

2) Lane Changing: In Fig. 6, we see the 5s trajectorycomputed in 2.3s in the lane changing scenario. The orangevehicle starts behind the blue one, but with a higher desiredspeed. To respect its desired speed while achieving lanechange, it passes the blue vehicle before changing lanes.

3) Overtaking: The five-second overtaking trajectorycomputed in 4.2s is presented in Fig. 7. We simply placea vehicle with high speed behind a slower vehicle. The costfunction is strongly penalizing deviation from the desiredspeed along the roadway axis and encouraging the vehiclesto end their trajectories in the right lane. This is sufficientto trigger an overtaking maneuver by the faster vehicle. Theslower vehicle slightly nudges towards the boundary of theroad to accommodate the overtaking vehicle’s trajectory.

4) Rich Autonomous Behavior: Our solver can handle costfunctions that depend on the decision variables of all theplayers ppp. However, we tested our solver with a cost functiononly dependent on the individual decision variables of eachplayer pν . In this simple setting, the open-loop Stackelbergequilibrium is trivial [2], [3]. The leader chooses his strategyfirst, ignoring collision constraints. The follower then selectsa strategy that has to account for the collision constraintsconsidering the leader’s trajectory as immutable. On thecontrary, our approach converges to trajectories that accountfor the individual objective of all players while sharing theresponsibility of avoiding collisions, even with “egoistic”cost functions. The conclusion from these experiments is thatsolving for Nash equilibrium apparently produces natural-looking trajectories.

Fig. 6. Lane changing scenario: the two vehicles successfully change lanes.

Fig. 7. Overtaking scenario: successful overtaking maneuver.

VII. CONCLUSIONS

We have introduced a new algorithm for finding Nashequilibrium trajectories. We demonstrated the speed androbustness of the solver on complex autonomous drivingscenarios including nonlinear and non-convex constraints.We have shown near-real-time performance for up to 3players. In a real-world driving application, replanning wouldbe performed as frequently as possible to give a feedbackpolicy in the sense of MPC. Parallelizing the computationof both the sparse matrix H and the gradient G shouldlead to large reductions in solution time, enabling true real-time performance in many realistic scenarios. Indeed, thesecomputations are decomposable across the number of playersand the number of time steps. We intend to exploit thisenticing property in future work.

The results we obtained from ALGAMES are promisingas they seem to let the vehicles share the responsibility foravoiding collisions, leading to seemingly natural trajectorieswhere players are able to negotiate complex, interactive traf-fic scenarios that are challenging for traditional, non-game-theoretic trajectory planners. For this reason, we believe thatthis solver could be a very efficient tool to generate trajec-tories in situations where the level of interaction betweenplayers is strong.

ACKNOWLEDGMENTS

This work was supported in part by DARPA YFA awardD18AP00064 and NSF NRI award 1830402.

REFERENCES

[1] P. Trautman and A. Krause, “Unfreezing the robot: Navigation indense, interacting crowds,” in 2010 IEEE/RSJ International Confer-ence on Intelligent Robots and Systems, (Taipei), pp. 797–803, IEEE,Oct. 2010.

[2] D. Sadigh, S. Sastry, S. A. Seshia, and A. D. Dragan, “Planningfor Autonomous Cars that Leverage Effects on Human Actions,” inRobotics: Science and Systems XII, Robotics: Science and SystemsFoundation, 2016.

[3] D. Sadigh, S. S. Sastry, S. A. Seshia, and A. Dragan, “Informationgathering actions over human internal state,” in 2016 IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS),(Daejeon, South Korea), pp. 66–73, IEEE, Oct. 2016.

[4] D. Fridovich-Keil, E. Ratner, J. C. Shih, A. D. Dragan, and C. J.Tomlin, “Iterative Linear Quadratic Approximations for NonlinearMulti-Player General-Sum Differential Games,” p. 8, 2019.

[5] A. Dreves and M. Gerdts, “A generalized Nash equilibrium approachfor optimal control problems of autonomous cars: A generalized Nashequilibrium approach for optimal control problems of autonomouscars,” Optimal Control Applications and Methods, vol. 39, pp. 326–342, Jan. 2018.

[6] J. F. Fisac, E. Bronstein, E. Stefansson, D. Sadigh, S. S. Sastry,and A. D. Dragan, “Hierarchical Game-Theoretic Planning for Au-tonomous Vehicles,” in 2019 International Conference on Roboticsand Automation (ICRA), (Montreal, QC, Canada), pp. 9590–9596,IEEE, May 2019.

[7] E. Schmerling, K. Leung, W. Vollprecht, and M. Pavone, “MultimodalProbabilistic Model-Based Planning for Human-Robot Interaction,”in 2018 IEEE International Conference on Robotics and Automation(ICRA), (Brisbane, QLD), pp. 1–9, IEEE, May 2018.

[8] R. Spica, D. Falanga, E. Cristofalo, E. Montijano, D. Scaramuzza, andM. Schwager, “A Real-Time Game Theoretic Planner for AutonomousTwo-Player Drone Racing,” arXiv:1801.02302 [cs], Jan. 2018.

[9] A. Liniger and J. Lygeros, “A Noncooperative Game Approach to Au-tonomous Racing,” IEEE Transactions on Control Systems Technology,pp. 1–14, 2019.

[10] A. Bressan, “Noncooperative Differential Games. A Tutorial,” Depart-ment of Mathematics, Penn State University, p. 81, 2010.

[11] T. Basar and G. J. Olsder, Dynamic noncooperative game theory,vol. 23. Siam, 1999.

[12] F. Facchinei and C. Kanzow, “Generalized Nash equilibrium prob-lems,” 4OR, vol. 5, pp. 173–210, Sept. 2007.

[13] J. H. Yoo and R. Langari, “Stackelberg Game Based Model ofHighway Driving,” in Volume 1: Adaptive Control; Advanced VehiclePropulsion Systems; Aerospace Systems; Autonomous Systems; BatteryModeling; Biochemical Systems; Control Over Networks; Control Sys-tems Design; Cooperativ, (Fort Lauderdale, Florida, USA), pp. 499–508, ASME, Oct. 2012.

[14] A. Britzelmeier, A. Dreves, and M. Gerdts, “Numerical solutionof potential games arising in the control of cooperative automaticvehicles,” in 2019 Proceedings of the Conference on Control and ItsApplications (W. S. Levine and R. Stockbridge, eds.), (Philadelphia,PA), pp. 38–45, Society for Industrial and Applied Mathematics, Jan.2019.

[15] M. Wang, Z. Wang, J. Talbot, and M. Schwager, “Game TheoreticPlanning for Self-Driving Cars in Competitive Scenarios,” p. 9.

[16] J. Morimoto and C. G. Atkeson, “Minimax Differential DynamicProgramming: An Application to Robust Biped Walking,” Advancesin neural information processing systems, pp. 1563–1570, 2003.

[17] J.-S. Pang and M. Fukushima, “Quasi-variational inequalities, general-ized Nash equilibria, and multi-leader-follower games,” ComputationalManagement Science, vol. 2, pp. 21–56, Jan. 2005.

[18] F. Facchinei and J.-S. Pang, “Exact penalty functions for generalizedNash problems,” Large-scale nonlinear optimization, pp. 115–126,2006.

[19] F. Facchinei and C. Kanzow, “Penalty methods for the solution of gen-eralized Nash equilibrium problems (with complete test problems),”p. 45, 2009.

[20] F. Facchinei, A. Fischer, and V. Piccialli, “Generalized Nash equi-librium problems and Newton methods,” Mathematical Programming,vol. 117, pp. 163–194, Mar. 2009.

[21] M. Fukushima, “Restricted generalized Nash equilibria and controlledpenalty algorithm,” Computational Management Science, vol. 8,pp. 201–218, Aug. 2011.

[22] D. P. Bertsekas, Constrained optimization and Lagrange multipliermethods. Academic press, 2014.

[23] J. Nocedal and S. Wright, Numerical optimization. Springer Science& Business Media, 2006.