# A Hamilton-Jacobi Formulation for Optimal Coordination of Heterogeneous Multiple Vehicle Systems

We present a method for optimal coordination of multiple vehicle teams when multiple endpoint configurations are equally desirable, such as seen in the autonomous assembly of formation flight. The individual vehicles' positions in the formation are not assigned a priori and a key challenge is to find the optimal configuration assignment along with the optimal control and trajectory. Commonly, assignment and trajectory planning problems are solved separately. We introduce a new multi-vehicle coordination paradigm, where the optimal goal assignment and optimal vehicle trajectories are found simultaneously from a viscosity solution of a single Hamilton-Jacobi (HJ) partial differential equation (PDE), which provides a necessary and sufficient condition for global optimality. Intrinsic in this approach is that individual vehicle dynamic models need not be the same, and therefore can be applied to heterogeneous systems. Numerical methods to solve the HJ equation have historically relied on a discrete grid of the solution space and exhibits exponential scaling with system dimension, preventing their applicability to multiple vehicle systems. By utilizing a generalization of the Hopf formula, we avoid the use of grids and present a method that exhibits polynomial scaling in the number of vehicles.

## Authors

• 4 publications
• 2 publications
• 6 publications
• ### Mobile Formation Coordination and Tracking Control for Multiple Non-holonomic Vehicles

This paper addresses forward motion control for trajectory tracking and ...
02/28/2019 ∙ by Xiuhui Peng, et al. ∙ 0

• ### Reachability as a Unifying Framework for Computing Helicopter Safe Operating Conditions and Autonomous Emergency Landing

We present a numeric method to compute the safe operating flight conditi...
05/16/2020 ∙ by Matthew R. Kirchner, et al. ∙ 0

• ### Physics-Based Freely Scalable Continuum Deformation for UAS Traffic Coordination

This paper develops a novel physics-inspired traffic coordination approa...
03/23/2019 ∙ by Hossein Rastgoftar, et al. ∙ 0

• ### Tukey Depths and Hamilton-Jacobi Differential Equations

The widespread application of modern machine learning has increased the ...
04/04/2021 ∙ by Martin Molina-Fructuoso, et al. ∙ 0

• ### Safe Coverage of Compact Domains For Second Order Dynamical Systems

Autonomous systems operating in close proximity with each other to cover...
11/15/2019 ∙ by Juan Chacon, et al. ∙ 0

• ### Deterministic particle flows for constraining SDEs

Devising optimal interventions for diffusive systems often requires the ...
10/25/2021 ∙ by Dimitra Maoutsa, et al. ∙ 0

• ### Optimal target assignment for massive spectroscopic surveys

Robotics have recently contributed to cosmological spectroscopy to autom...
05/18/2020 ∙ by Matin Macktoobian, et al. ∙ 0

##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## I Introduction

Multi-robot motion planning presents several challenges, and key among them are trajectory planning and formation control, as well as the assignment of vehicles to goal states. When the assignment of vehicles to goal states is made beforehand, many methods have been proposed. These include control theory approaches [40, 11], graph-based techniques [14], and optimization methods [27, 2]. Approximations to assist in dimensionality reductions have also been proposed, such as sequential methods [8] and hybrid approaches [10, 37].

When the goal states of multi-vehicle teams are not set a priori, it is necessary to determine which vehicle to allocate to which terminal goal. There is a large body of literature developing allocation and assignment algorithms [5], and applying these methods to engineering problems has appeared in diverse forms including sensor coverage [26], weapon-target assignment [1], or network routing [24]. Dynamics are not considered in this body of work, which commonly study worst-case system performance under equilibrium conditions using a game-theoretical framework [38, 25, 21]. Without consideration of dynamics, these methods have limited applicability to vehicle coordination problems and there exists a technical gap relating vehicle-goal allocation and motion planning, as most prior work assumes each is done independently. The critical dependency between assignment and trajectory planning is illustrated in Figure 1.

There have been recent attempts to close this gap as in [17]. But this work only evaluates discrete spatial waypoints on a rectangular grid without considering vehicle dynamics, and therefore the paths generated are not feasible for most physical systems. Morgan et al. [31]

generalizes this concept to linear, discrete-time dynamics and constructs a non-linear programming problem that is solved with sequential convex programming in conjunction with an auction algorithm

[5]. However, both [31] and [17] restrict the cost function to one of additive individual vehicle weights (similar to the cost function presented in [5]), which excludes many vehicle optimization problems of interest, including assembling the desired formation shape in minimal time.

We present how the optimal assignment and trajectory can be found simultaneously from the viscosity solution to a single Hamilton–Jacobi PDE, a necessary and sufficient condition for optimality. We assume the vehicles can each have unique linear dynamics. Remarkably, we show that the global viscosity solution can be found as the solution to a linear bottleneck assignment problem (LBAP) [15]. This fact can be utilized to construct a level set method that has polynomial computational scaling with the number of vehicles and still ensures a global optimum.

Our formulation has a close relation to reachability analysis which can be useful for the design and analysis of heterogeneous systems. When the dynamics of some vehicles differ greatly from the rest, some formations may not be feasible (e.g. a slow-moving vehicle attempting to join a formation with a much faster vehicle). The zero sub-level set of the viscosity solution of a related HJ PDE defines an implicit surface representation of the backwards reachable set [28], and from this one determines whether the formation can be achieved given the unique collection of vehicle dynamics.

Traditionally, numerical solutions to HJ equations require a dense, discrete grid of the solution space [33, 30]

. Computing the elements of this grid scales poorly with dimension and has limited use for problems with dimension greater than four. The exponential dimensional scaling in optimization is sometimes referred to as the “curse of dimensionality

[4, 3]. A new result in [9] discovered a numerical solution based on the Hopf formula [18] that does not require a grid and can be used to efficiently compute solutions of a certain class of Hamilton–Jacobi PDEs. However, that only applies to systems with time-independent Hamiltonians of the form , and has limited use for general linear control problems. Recently, the classes of systems were expanded upon and generalizations of the Hopf formula are used to solve optimal linear control problems in high-dimensions [19] and differential games as applied to multi-vehicle, collaborative pursuit-evasion problems [20].

The key contributions of this paper are to formulate the multi-vehicle coordination problem as a single Hamilton–Jacobi equation, show that the solution of which can be found from an LBAP, and use the generalized Hopf formula to simultaneously solve for optimal trajectory, vehicle control, and goal assignment. By utilizing the generalized Hopf formula, we create a level-set method to find the viscosity solution of the HJ in a computationally efficient manner.

We introduce a multi-vehicle dynamical system and formulate the coordination problem in Section II. Section III formulates the optimal vehicle coordination problem as the viscosity solution to a single Hamilton–Jacobi PDE. Section IV constructs a level-set method for the multi-vehicle coordination problem and utilizes the generalized Hopf formula as a computational tool to find the viscosity solution to the HJ PDE of the joint, mutli-vehicle system. Section V shows the method as applied to several examples including a case of planar motion planning with 4 vehicles. This example shows how assignment and path planning cannot be decoupled and even if a single vehicle has a different initial condition, it may result in a different assignment for optimal coordination.

## Ii Problem Formulation

We consider a system that consists of vehicles and each vehicle, , has linear dynamics

 (1)

for , where is the system state and is the control input, constrained to a convex admissible control set

. We add the assumption that no eigenvalue of matrix

has a strictly positive real part. We let denote a state trajectory for vehicle that evolves in time with measurable control sequence , according to starting from initial state at . The trajectory is a solution of in that it satisfies almost everywhere:

 ddsγi(s;xi,α(⋅)) =Aiγi(s;xi,αi(⋅))+Biαi(s), γi(0;xi,αi(⋅)) =xi.

### Ii-a Multi-Vehicle Model

For the set of

vehicles, we construct a joint state space with state vector

and control which is written as follows

 ˙x=⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣˙x1⋮˙xi⋮˙xN⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦ =⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣A1⋯0⋱⋮Ai⋮⋱0⋯AN⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣x1⋮xi⋮xN⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦ +⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣B1⋯0⋱⋮Bi⋮⋱0⋯BN⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦⎡⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢⎣α1⋮αi⋮αN⎤⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦ ⟹˙x =Ax+Bα, (2)

We reiterate the above definition of the joint state space with the fact that when quantities , , , , and appear without subscripts, they refer to the joint system in , and when subscripts are used, they refer to a specific individual vehicle as defined in .

### Ii-B Vehicle Coordination

We assume there exists a set of goals, and for each vehicle we associate closed convex sets with the understanding that means the vehicle is assigned to goal . Our goal is to make sure that we have one vehicle at each goal, but it does not matter which vehicle is at each goal. This goal can be expressed by the requirements that the multi-vehicle state, , belongs to the following goal set

 Θ:={x∈Rn:∃σ∈SNs.t.∀i∈V,xi∈Ωi,σ(i)}, (3)

where denotes the set of all permutations of . We represent implicitly with the function such that

 Θ={x∈Rn|J(x)≤0}, (4)

and use it to construct a cost functional for the system trajectory , given terminal time as

 R(t,x,α(⋅))=∫t0C(α(s))ds+J(γ(t;x,α(⋅))), (5)

where the function represents the rate that cost is accrued over time. The value function is defined as the minimum cost, , among all admissible controls for a given initial state with

 φ(x,t)=infα(⋅)∈AR(t,x,α(⋅)). (6)

## Iii Hamilton–Jacobi Equations For Optimal Coordination

The value function in satisfies the dynamic programming principle [6, 12] and also satisfies the following initial value Hamilton–Jacobi (HJ) equation with being the viscosity solution of

 ∂φ∂s(x,s)+H(s,x,∇xφ(x,s)) =0, (7) φ(x,0) =J(x),

for , where the Hamiltonian is defined by

 H(s,x,p)=−x⊤A⊤p+supα∈Rm{⟨−Bα,p⟩−C(α)}, (8)

where in denotes the costate. We denote by the costate trajectory that can be shown to satisfy almost everywhere:

 ddsλ(s;x,α(⋅)) =∇xf(γ(s;x,α(⋅)),s)⊤λ(s;x,α(⋅)) λ(t;x,α(⋅)) =∇xφ(γ(t;x,α(⋅))),

with initial costate denoted by . With a slight abuse of notation, we will hereafter use to denote , when the initial state and control sequence can be inferred through context with the corresponding state trajectory, .

### Iii-a System Hamiltonian

We consider a time-optimal formulation with

 C(α)=IA(α),

where

is the characteristic function of the set of admissible controls and is defined by

 IA(α)={0ifα∈A+∞otherwise.

In this case, the integral term in disappears (as long as remains in and is simply the value of at the terminal time, . For this problem

 φ(x,t)=infα(⋅)∈AJ(γ(t;x,α(⋅)))≤0

means that there exists a feasible trajectory for the vehicles the ends at the state ; whereas means that such trajectories do not exist under the system dynamics and initial conditions. Since each vehicle has independent dynamics, the Hamiltonian in is of the form

 H(s,x,p)=∑iHi(s,xi,pi), (9)

where each vehicle’s Hamiltonian is given by

 Hi(s,xi,pi)= −x⊤iA⊤ipi (10) +supαi∈Rmi{⟨−Biαi,pi⟩−IAi(αi)}.

### Iii-B Linear Bottleneck Assignment

Our goal is solving the Hamilton–Jacobi PDE in , and any that satisfies is, in general, non-convex and presents numerical challenges. We show that we can overcome this challenge by alternatively solving for the global value function with the following linear bottleneck assignment problem [15]:

 φ(x,t)=minσ∈SNmaxi∈Vϕi,σ(i)(xi,t), (11)

where is the viscosity solution to

 ∂ϕi,j∂s(xi,s)+Hi(s,x,∇xϕi,j(xi,s)) =0, (12) ϕi,j(xi,0) =Ji,j(xi),

with Hamiltonian defined by . The function is an implicit surface representation of such that

 Ωi,j={xi∈Rni|Ji,j(xi)≤0}. (13)

The solution to can be found from using the appropriate linear bottleneck assignment algorithms (for example [7, Section 6.2]), requiring only evaluations of lower dimensional viscosity solutions and avoiding computation involving all permutations of vehicle-goal pairs. Also of note is that if each HJ equation has convex initial data, it enables the use of computationally efficient techniques that can guarantee convergence to the appropriate viscosity solution. Additionally, since each of the solutions are independent, the solutions can be computed in parallel.

We introduce a set of mild regularity assumptions that guarantee Hamilton–Jacobi equations have a unique viscosity solution [41, Chapter 7, p. 63]:

1. Each Hamiltonian

 [0,t]×Rni×Rni∋(s,xi,pi)↦Hi(s,xi,pi)∈R,∀i

is continuous.

2. There exists a constant such that for all and for all , the following inequalities hold

 ∣∣Hi(s,xi,p′)−Hi(s,xi,p′′)∣∣≤ρi(xi)∥∥p′−p′′∥∥,∀i

and

 |Hi(s,xi,0)|≤ρi(xi),∀i

with .

3. For any compact set there exists a constant such that for all and for all the inequality holds,

 ∣∣Hi(s,x′,pi)−Hi(s,x′′,pi)∣∣≤μi(pi)∥∥x′−x′′∥∥,

with .

4. Each terminal cost function

 Rni∋xi↦Ji,j(xi)∈R,∀i,j

is continuous.

###### Lemma 1

If each vehicle Hamiltonian, , meets assumptions 1-3, then the Hamiltonian defined in also meets assumptions 1-3.

The proof is given in the appendix.

###### Theorem 2

Under assumptions 1-4, is a unique viscosity solution to for all , and there exists a that satisfies such that with the Hamiltonian given by , is a viscosity solution to .

We will prove the theorem constructively by proposing a particular given as

 J(x)=minσ∈SNJσ(x), (14)

with

 Jσ(x):=maxi∈VJi,σ(i)(xi), (15)

where is the implicit representation of , defined in , with . We see that for any when which implies . We also see if there exists an such that , then and implies . Therefore the proposed in satisfies and is an implicit surface representation of the set . Furthermore, since by assumption each is continuous, then is convex as the max of a finite number of continuous functions is also continuous. This implies is continuous as the minimum of a finite number of coninuous functions is continuous. Therefore satisfies assumption 4.

From Lemma 1, the system Hamiltonian defined in meets assumptions 1-3 and implies that has a unique viscosity solution, denoted as , when the initial cost function is given by [41, Theorem 8.1, p. 70]. The uniqueness of the solution implies that the viscosity solution is equivalent to the value function in . It follows that

 φHJ(x,t)=infα(⋅)∈AJ(γ(t;x,α(⋅))), (16)

since when . Denoting by as the control that optimizes , we have

 φHJ(x,t)=J(γ(t;x,α∗(⋅))).

Substituting and we have

 φHJ(x,t) =minσ∈SNmaxi∈VJi,σ(i)([γ(t;x,α∗(⋅))]i), (17)

where we use the notation to represent the -th block of the vector . Likewise has a unique viscosity solution,, for each with initial cost given , and as such

 ϕi,σ(i)(xi,t) =Ji,σ(i)(γi(t;xi,α∗i(⋅))), (18)

for each and with denoting the control that optimizes . Note that if it would contradict that is the optimal control. Also if , then it would contradict that is the optimal control of the entire system. Therefore, and our value function in becomes

 φHJ(x,t)=minσ∈SNmaxi∈Vϕi,σ(i)(xi,t),

and we arrive at our result.

## Iv A Level Set Method with the Generalized Hopf Formula

First, we introduce the Fenchel–Legendre transform of a convex, proper, lower semicontinuous function , denoted as , and is defined as [16]

 g⋆(p)=supy∈Rℓ{⟨p,y⟩−g(y)}. (19)

We propose to find the viscosity solutions of using the generalized Hopf formula.

###### Theorem 3

Each viscosity solution of can be found from the formula

 ϕi,j(xi,t)= −minpi∈Rni{J⋆i,j(e−tA⊤ipi), (20) +∫t0ˆHi(s,pi)ds−⟨xi,pi⟩}

with

 ˆHi(s,pi)=% supα∈Ai{⟨−e−sAiBiαi,pi⟩}. (21)

Proceeding similar to [20], we apply a change of variables to with

 zi(s)=e−sAixi(s), (22)

which results in the following system

 ddszi(s)=e−sAiBiαi(s). (23)

By utilizing this change of variables, we construct an equivalent HJ equation

 ∂~ϕi,j∂s(zi,s)+ˆHi(s,∇z~ϕi,j(zi,s)) =0, (24) ~ϕi,j(z,0) =Ji,j(etAzi).

From Lemma 5 (given in the appendix), meets assumption 1-3, and by composition rule is continuous [39, Theorem 4.7] and meets assumption 4. Therefore has a unique viscosity solution that is equivalent to the cost functional

 ~ϕi,j(zi,t)=infαi(⋅)∈AiJi,j(etAξi(t;zi,αi(⋅))),

where is a solution trajectory that satisfies almost everywhere. Since

 zi(t)=e−tAixi(t),

we have

 ~ϕi,j(zi,t) =% infαi(⋅)∈AiJi,j(etAξi(t;zi,αi(⋅))) =ϕi,j(xi,t),

noting that since by the transform implies at . Thus, and we can find by finding the viscosity solution to .

Since is assumed closed and convex, this implies is convex and by assumption 4 is continuous in . By assumption 1, is continuous in , therefore gives an exact, point-wise viscosity solution for [23, Section 5.3.2, p. 215]. Formula shows that we can compute a viscosity solution to by solving a finite dimensional optimization problem. This avoids constructing a discrete spatial grid, and is numerically efficient to compute even when the dimension of the state space is large. Additionally, no spatial derivative approximations are needed with Hopf formula-based methods, and this eliminates the numeric dissipation introduced with the Lax–Friedrichs scheme [34], which is necessary to maintain numeric stability in grid-based methods.

### Iv-a Numerical Optimization of the Hopf Formula

We transcribe the Hopf formula into a non-linear programming problem by approximating the integral in with an -point quadrature rule sampled on the time grid

 πN={sk:k=0,…,N},

with and corresponding weights . Additionally, we make a simple, invertible change of variable and substituting into results in the following unconstrained optimization problem that solves :

 (25)

with now being defined as

 ˆHi(s,~pi)=supαi∈Ai{⟨−Biαi,esA⊤i~pi⟩}. (26)

The variable transformation is done so that when the matrix has at least one eigenvalue with strictly negative real part, we avoid computation of which would have exponentially unstable poles since we are evaluating the matrix exponential of . This divergence would cause sensitivity in the evaluation of the Hopf formula in with respect to small changes of . By utilizing the variable transformation and optimizing , we avoid this sensitivity.

###### Remark 4

Often the expression in is known in closed form and we present one such common case. Recall that denotes the Fenchel–Legendre transform of a function defined in , and suppose is the closed convex set defined as

 Ai:={u:∥u∥≤1},

where is any norm. Then defines a norm, which we denote with , which is the dual norm to [16]. From this we write as

 ˆHi(s,~pi)=∥∥−B⊤iesA⊤i~pi∥∥∗. (27)

### Iv-B Time-Optimal Control to a Goal Set

The task of determining the control that drives the system into in minimal time can be determined by finding the smallest such that

 φ(x,t)≤0. (28)

When the system is constrained controllable, then the set of times such that is reachable with respect to contains the open interval [13]. This insures that if is outside the set at time , then and there exists a time such that for all . This implies standard root-finding algorithms can be used to find . As noted in [19], we solve for the minimum time to reach the set by constructing a newton iteration, starting from an initial guess, , with

 tk+1=tk−φ(x,tk)∂φ∂t(x,tk), (29)

where is the solution to at time . The value function must satisfy the HJ equation

 ∂φ∂t(x,tk)=−H(s,x,∇xφ(x,tk)),

where and each is the argument of the minimizer in . We iterate until convergence at the optimal time to reach, which we denote as . This process is summarized in Algorithm 1.

The optimal control and trajectory for each vehicle is found directly from the necessary conditions of optimality established by Pontryagin’s principal [35] by noting the optimal trajectory must satisfy

 ddsγ∗i(s;xi,α∗i(⋅)) =−∇pH(s,xi(s),λ∗i(s)) =Aiγi(s;xi,α∗i(⋅)) +Bi∇p∥∥−B⊤iλ∗i(s)∥∥∗,

where is the optimal costate trajectory and is given by

 λ∗i(s)=e−(t∗−s)A⊤i~p∗i.

This implies our optimal control is

 α∗i(s)=∇p∥∥−B⊤ie−(t∗−s)A⊤i~p∗i∥∥∗, (30)

for all time , provided the gradient exists.

## V Results

We present results for two examples. For the first we choose a toy example of two vehicles, each with differing single integrator dynamics. The purpose of this example is that it allows easy visualization of the level set propagation and the solution can be verified by comparing with methods such as [29], which cannot be applied to the higher dimensional second example to follow. The LBAP was solved using [7, Algorithm 6.1] and the optimization used sequential quadratic programming [32, Chapter 18] with initial conditions for each vehicle. To prevent a singular control condition, a slight smoothing was applied to the Hamiltonian in and using [36] with parameter .

### V-a Toy Problem

We present a two vehicle problem where the dynamics are for the first vehicle and for the second vehicle. The states are the linear position of each respective vehicle. The control is bounded by . The goal states are for and for . The level set contours of the joint space are shown in Figure V-A, for ten time samples equally spaced on . Notice this seemingly simple heterogeneous system can give rise to counterexamples for the assignment formulations given in [31] where the sum of the vehicle distances are used as the assignment metric and [17] where the time of arrival of vehicles was used. Take the initial state , which is shown on Figure 2. For the time metric, the the assignment gives , while the assignment gives , indicating is the clear choice. However, is the global optimum in this example, as both vehicles reach their desired goal states in a time of , as the zero level set of the global value function intersects the point at that time, giving the minimum time-to-reach. For the distance metric, the misassignment is more pronounced, with the assignment giving as compared to the assignment gives .

### V-B Planar Motion

We choose for dynamics with state , where is spatial position of a robot and is the velocity and

 Ai=⎡⎢ ⎢ ⎢⎣0010000100−10000−1⎤⎥ ⎥ ⎥⎦,Bi=⎡⎢ ⎢ ⎢⎣00001001⎤⎥ ⎥ ⎥⎦,

for each vehicle . The control is constrained to lie in the set . The robots are tasked with reaching the goal formation and coming to rest, in minimum time. The goal sets each have radius of and the centers of the goals are located spatially at , , , and . Since the 2-norm is self-dual, the Hamiltonian for each vehicle is

 ˆHi(s,~pi)=∥∥−B⊤iesA⊤i~pi∥∥2.

Since contains eigenvalues with negative real part, we optimize using the variable transformation of Section IV-A. The initial conditions of the vehicles were

 x1(0) =(3,−10,−1,1)⊤, x2(0) =(−1,−12,1,1)⊤, x3(0) =(−4,−11,2,−1)⊤, x4(0) =(6,−13,−1,−1)⊤.

Only iterations of were needed to solve for the minimum time to reach the goal formation, which was found to be . Figures (a)a and (b)b show the optimal paths found from . Figure (c)c shows the optimal paths when the initial condition for vehicle 4 was changed to and a different optimal assigment results.

## Vi Conclusions and Future Work

We presented how to formulate vehicle coordination problems with unknown goal assignments as the viscosity solution to a single Hamilton–Jacobi equation. We show the solution of this single HJ PDE is equivalent to decomposing the problem and performing a linear bottleneck assignment using the viscosity solutions of independent single-vehicle problems. This allows quadratic computational scaling in the number of vehicles. Finally, a level set method based on the Hopf formula was presented for efficient computation of the vehicle value functions, in which each can be computed in parallel. The Hamilton–Jacobi formulation presented has other advantages for multi-robot systems, such as the compensation of time delays which can be induced in several ways including computation, sensing, and inter-robot communication. See for example [22].

Future work includes to expand the class of allowable vehicle dynamics from general linear models to certain types of nonlinear dynamics and to allow for dependencies between vehicles both in the dynamics models and in the cost functional.

## Appendix

###### Lemma 5

Given that satisfies assumptions 1-3, then also satisfies assumption 1-3.

From assumption 1, is continuous by composition rule [39, Theorem 4.7] and since does not depend on state, it therefore trivially meets assumption 3. It remains to show that meets assumption 2. We know assumption 2 holds for all and for all , therefore the following holds

 ∣∣Hi(s,xi,e−sA⊤ip′)−Hi(s,xi,e−sA⊤ip′′)∣∣ ≤ci(1+∥xi∥)∥∥e−sA⊤ip′−e−sA⊤ip′′∥∥ ≤ci(1+∥xi∥)∣∣λmax(e−sA⊤i)∣∣∥∥p′−p′′∥∥ =c(1+∥xi∥)∥∥p′−p′′∥∥.

When we have

 ∣∣Hi(s,0,e−sA⊤ip′)−Hi(s,0,e−sA⊤ip′′)∣∣ =∣∣ˆHi(s,p′)−ˆHi(s,p′′)∣∣ ≤c∥∥p′−p′′∥∥≤c(1+∥xi∥)∥∥p′−p′′∥∥,

for any . And since for all , it follows that assumption 2 is met.

Proof of Lemma 1. The sum of continuous functions is also continuous [39, Theorem 4.9], therefore assumption 1 is met. To prove assumption 2, we first write, for all ,

 ∣∣H(s,x,¯¯¯p′)−H(s,x,¯¯¯p′′)∣∣ ≤∑i∣∣Hi(s,xi,¯¯¯p