Game theoretic controller synthesis for multi-robot motion planning\\ Part I : Trajectory based algorithms
Minghui Zhu
Michael Otte
Pratik Chaudhari
Emilio Frazzoli \thanks{M. Zhu is with the Department of Electrical Engineering
Pennsylvania State University
201 Old Main
University Park
PA
16802.\newline Email:~\href{mailto:muz16@psu.edu
We consider a class of multi-robot motion planning problems where each robot is associated with multiple objectives and decoupled task specifications. The problems are formulated as an open-loop non-cooperative differential game. A distributed anytime algorithm is proposed to compute a Nash equilibrium of the game. The following properties are proven: (i) the algorithm asymptotically converges to the set of Nash equilibrium; (ii) for scalar cost functionals, the price of stability equals one; (iii) for the worst case, the computational complexity and communication cost are linear in the robot number.
Zhu.Otte.ea.ICRA14.tex
\documentclass[letterpaper, 10pt, conference]{ieeeconf}

\IEEEoverridecommandlockouts
\overrideIEEEmargins

\usepackage{amsmath,amssymb,pstricks,color,dsfont}
%\usepackage[sort,numbers]{natbib}
\usepackage{graphicx}
\usepackage{psfrag,graphicx,epsfig}
\usepackage{xspace}
\usepackage{subfig}
\usepackage{float}
\usepackage{placeins}
\usepackage{multirow}
\usepackage{pgf,tikz}

\usepackage[vlined,linesnumbered,boxruled]{algorithm2e}

\usepackage[bookmarks=true]{hyperref}
\hypersetup
{
colorlinks=true,
linkcolor=red, citecolor=blue, filecolor=magenta, urlcolor=blue
%linkcolor=black, citecolor=black, filecolor=black, urlcolor=black
}
\usepackage{url, cite}

\usepackage{array}

\newcolumntype{x}[1]{%
>{\centering\hspace{0pt}}p{#1}}%

% save space
\newcommand{\algsize}{\footnotesize}
\setlength{\floatsep}{0.1in}
\setlength{\textfloatsep}{0in}
\setlength{\intextsep}{0.1in}
\setlength{\belowcaptionskip}{0.05in}
\setlength{\abovecaptionskip}{0.1in}
\setlength{\abovedisplayskip}{0.05in}
\setlength{\belowdisplayskip}{0.05in}

\newcommand{\ul}{\underline }
\newcommand{\ulx}{\underline x}
\newcommand{\ep}{\epsilon}
\newcommand{\sgm}{\sigma}
\newcommand{\Dt}{\Delta}
\newcommand{\dt}{\delta}
\newcommand{\bt}{\beta}
\newcommand{\al}{\alpha}
\newcommand{\lbd}{{\lambda}}
\newcommand{\kth}{$k^{th}$-order }
\newcommand{\om}{\omega}
\newcommand{\homega}{\hat{\omega}}
\newcommand{\bx}{{\bf x}}
\newcommand{\be}{{\bf e}}
\newcommand{\bz}{{\bf z}}
\newcommand{\br}{{\bf r}}
\newcommand{\bv}{{\bar v}}
\newcommand{\bw}{{\bf w}}
\newcommand{\bbu}{{\bf u}^{(k)} }
\newcommand{\bbx}{{\bf x}^{(k)} }
\newcommand{\bbz}{{\bf z}^{(k)} }
\newcommand{\by}{{\bf y} }
\newcommand{\bby}{{\bf y} }
\newcommand{\bxc}{{{\bf x}_c}}
\newcommand{\bu}{{\bf u}}
\newcommand{\bxk}{{\bf x}_{(k)} }
\newcommand{\buk}{{\bf u}_{(k)} }
\newcommand{\col}{\mbox{col}}
\newcommand{\px}{{\partial x}}
\newcommand{\pe}{{\partial e}}
\newcommand{\pxc}{{\partial x_c}}
%\newcommand{\pf}{{\partial f}}
\newcommand{\pa}{{\partial a}}
\newcommand{\pv}{{\partial v}}
\newcommand{\pu}{{\partial u}}
\newcommand{\pr}{{\partial r}}
\newcommand{\pw}{{\partial w}}
\newcommand{\pfc}{{\partial f_c}}
\newcommand{\phc}{{\partial h_c}}
\newcommand{\ph}{{\partial h}}
\newcommand{\pk}{{\partial k}}
\newcommand{\pg}{{\partial g}}
\newcommand{\pz}{{\partial z}}
\newcommand{\nnum}{\nonumber}


\newcommand{\EQ}{\begin{eqnarray}}
\newcommand{\EN}{\end{eqnarray}}
\newcommand{\EQQ}{\begin{eqnarray*}}
\newcommand{\ENN}{\end{eqnarray*}}
\newcommand{\defi}{\stackrel{\mbox{\small def}}{=}}
\newcommand{\IEEE}{{\it IEEE Transactions on Automatic Control}}



\newcommand{\bremark}{\begin{remark} \begin{rm} }
\newcommand{\eremark}{ \end{rm} \rule{1mm}{2mm}
\end{remark} }
\newcommand{\btheorem}{\begin{theorem} \begin{rm} }
\newcommand{\etheorem}{ \end{rm} \rule{1mm}{2mm}
\end{theorem} }
\newcommand{\blemma}{\begin{lemma} \begin{rm} }
\newcommand{\elemma}{ \end{rm} \rule{1mm}{2mm}
\end{lemma} }
\newcommand{\bcorollary}{\begin{corollary} \begin{rm} }
\newcommand{\ecorollary}{ \end{rm} \rule{1mm}{2mm}
\end{corollary} }
\newcommand{\bdefinition}{\begin{definition}\begin{rm} }
\newcommand{\edefinition}{ \end{rm} \rule{1mm}{2mm}
\end{definition} }
\newcommand{\bproposition}{\begin{proposition} \begin{rm} }
\newcommand{\eproposition}{ \end{rm} \rule{1mm}{2mm}
\end{proposition} }
\newcommand{\bexample}{\begin{example} \begin{rm} }
\newcommand{\eexample}{ \end{rm} \rule{1mm}{2mm}
\end{example} }
\newcommand{\basm}{\begin{assumption} \begin{rm}}
\newcommand{\easm}{\end{rm} %\rule{1mm}{2mm}
\end{assumption}}


%%% List of new commands useful for shortand
\newcommand{\real}{\mathds{R}}
\newcommand{\posreal}{\mathds{R}_{\ge 0}}
\newcommand{\complex}{\mathds{C}}
\newcommand{\integer}{\mathds{Z}}
\renewcommand{\natural}{\mathds{N}}
\newcommand{\natzero}{\mathds{N}_0}
\newcommand{\sphere}{\mathds{S}}
\newcommand{\rational}{\mathds{Q}}

\newcommand{\QP}{\operatorname{QP}}
\newcommand{\optresponse}{\operatorname{OR}}

\newcommand{\fder}[2]{\frac{d #1}{d #2}}
\newcommand{\tfder}[2]{\tfrac{d #1}{d #2}}
\newcommand{\pder}[2]{\frac{\partial #1}{\partial #2}}
\newcommand{\tpder}[2]{\tfrac{\partial #1}{\partial #2}}
\newcommand{\pmat}[1]{\begin{pmatrix} #1 \end{pmatrix}}
\newcommand{\bmat}[1]{\begin{bmatrix} #1 \end{bmatrix}}

\newcommand{\Ac}{\mathcal{A}}
\newcommand{\CC}{\mathcal{C}}
\newcommand{\DD}{\mathcal{D}}
\newcommand{\EE}{\mathcal{E}}
\newcommand{\FF}{\mathcal{F}}
\newcommand{\GG}{\mathcal{G}}
\newcommand{\HH}{\mathcal{H}}
\newcommand{\PP}{\mathcal{P}}
\newcommand{\QQ}{\mathcal{Q}}
\newcommand{\LL}{\mathcal{L}}
\newcommand{\NN}{\mathcal{N}}
\newcommand{\inputseq}{\mathbf{u}}
\newcommand{\stateseq}{\mathbf{x}}


%% geometric notions
\newcommand{\diam}{\operatorname{diam}}
\newcommand{\co}{\operatorname{co}}
\newcommand{\aff}{\operatorname{aff}}
\newcommand{\dist}{\operatorname{dist}}


%% mathematical constructs: \setdef, \until, etc
\newcommand{\until}[1]{\{1,\dots, #1\}}
\newcommand{\subscr}[2]{#1_{\textup{#2}}}
\newcommand{\supscr}[2]{#1^{\textup{#2}}}
\newcommand{\setdef}[2]{\{#1 \; | \; #2\}}
\newcommand{\bigsetdef}[2]{\big\{#1 \; | \; #2\big\}}
\newcommand{\Bigsetdef}[2]{\Big\{#1 \; \big| \; #2\Big\}}
\newcommand{\map}[3]{#1: #2 \rightarrow #3}
\newcommand{\setmap}[3]{#1: #2 \rightrightarrows #3}
\newcommand{\id}{\operatorname{id}}
\newcommand{\diag}[1]{\operatorname{diag}(#1)}
\newcommand{\setofsetvalued}[2]{\mathcal{SC}(#1,#2)}
\newcommand{\spn}{\operatorname{span}}

\newcommand{\FL}{\operatorname{FL}}
\newcommand{\argmax}{\operatorname{argmax}}
\newcommand{\trace}{\operatorname{tr}}

\newtheorem{theorem}{\bf Theorem}[section]
\newtheorem{lemma}{\bf Lemma}[section]
\newtheorem{definition}{\bf Definition}[section]
\newtheorem{remark}{\bf Remark}[section]
\newtheorem{corollary}{\bf Corollary}[section]
\newtheorem{proposition}{\bf Proposition}[section]
\newtheorem{example}{\bf Example}[section]
\newtheorem{assumption}{\bf Assumption}[section]


% Procend
\newcommand\oprocendsymbol{\hbox{$\bullet$}}
\newcommand\oprocend{\relax\ifmmode\else\unskip\hfill\fi\oprocendsymbol}
\def\eqoprocend{\tag*{$\bullet$}}

%% missing parts, action items
\newcommand{\todo}[1]{\par\noindent{\raggedright\textsc{\color{red}#1}%
    \par\marginpar{{\Large\color{red}$\star$}}}}

\newcommand{\margin}[1]{\marginpar{\color{red}\tiny\ttfamily#1}}
\setlength{\marginparwidth}{0.5in}
\newcommand{\efmargin}[2]{{\color{teal}#1}\marginpar{\color{teal}\footnotesize\raggedright #2 }}

\definecolor{darkgreen}{rgb}{0,0.6,0}
\definecolor{brown}{rgb}{0.65,0.16,0.16}
\newcommand{\pcmargin}[2]{{\color{darkgreen} #1}\marginpar{\tiny\noindent{\raggedright{\color{darkgreen} [PC]}\color{brown}{ #2} \par}}}

\date{}

% the following helps when one of the packages breaks on Otte's latex install
\providecommand{\IncMargin}[1]{}

\begin{document}

\title{Game theoretic controller synthesis for multi-robot motion planning\\
Part I : Trajectory based algorithms}

\author{Minghui Zhu, Michael Otte, Pratik Chaudhari, Emilio Frazzoli \thanks{M. Zhu is with the Department of Electrical Engineering, Pennsylvania State University, 201 Old Main, University Park, PA, 16802.\newline Email:~\href{mailto:muz16@psu.edu}{muz16@psu.edu}.\newline M. Otte, P. Chaudhari and E. Frazzoli are with the Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, 77 Massachusetts Avenue, Cambridge MA, 02139.\newline Email:~\mbox{\href{mailto:ottemw@mit.edu}{ottemw@mit.edu}}, \href{mailto:pratikac@mit.edu}{pratikac@mit.edu}, \href{mailto:frazzoli@mit.edu}{frazzoli@mit.edu}.\newline This research was supported in part by ONR Grant \#N00014-09-1-0751 and the Michigan/AFRL Collaborative Center on Control Sciences, AFOSR grant \#FA 8650-07-2-3744.}}

\maketitle

\begin{abstract} We consider a class of multi-robot motion planning problems where each robot is associated with multiple objectives and decoupled task specifications. The problems are formulated as an open-loop non-cooperative differential game. A distributed anytime algorithm is proposed to compute a Nash equilibrium of the game. The following properties are proven: (i) the algorithm asymptotically converges to the set of Nash equilibrium; (ii) for scalar cost functionals, the price of stability equals one; (iii) for the worst case, the computational complexity and communication cost are linear in the robot number.
\end{abstract}

\section{Introduction}

Robotic motion planning is a fundamental problem where a control sequence is found to steer a robot from the initial state to the goal set, while enforcing the environmental rules. It is well-known that the problem is computationally challenging~\cite{Reif:79}. The situation is even worse for multi-robot motion planning since the computational complexity exponentially grows as the robot number.

%For example, the problem of generalized piano mover is shown to be PSPACE-hard~\cite{Reif:79}.

For multi-robot motion planning, non-cooperative game theoretic controller synthesis is interesting in two aspects: \emph{descriptive} and \emph{perspective}. From the descriptive point of view, Nash equilibrium is desirable in inherently competitive scenarios. More specifically, Nash equilibrium characterizes the stable scenarios among inherently self-interested players where none can benefit from unilateral deviations. From the perspective point of view, non-cooperative game theoretic learning holds the promise of providing computationally efficient algorithms for multi-robot controllers where the robots are assumed to be self-interested. Although Nash equilibrium may not be socially optimal, game theoretic approaches remain useful when the computational efficiency dominates.

%One example is autonomous intersections in urban transportation networks. No selfish vehicle can go through the intersection more quickly via deviating from its own Nash equilibrium strategy if other vehicles stick to their own Nash equilibrium strategies.

There have been limited results on rigorous analysis of game theoretic controller synthesis for multi-robot motion planning. The paper~\cite{LaValle.Hutchinson:98} tackles multi-robot motion planning in the framework of feedback differential games. However, it lacks of the rigorous analysis of the algorithm's convergence and computational complexity. In addition, static game theory has been used to synthesize distributed control schemes to steer multiple vehicles to stationary and meaningful configurations; e.g., in~\cite{Zhu.Martinez:12} for optimal sensor deployment, in~\cite{Arsie.Savla.ea:TAC09} for vehicle routing and in~\cite{GA-JRM-JSS:07a} for target assignment.

%In this paper, we further utilize differential games to control multiple robots with heterogeneous objectives and complex task specifications.

%\emph{Contributions.} In this paper and the companion one~\cite{Zhu.Mueller.ea:14}, we employ game theory and incremental sampling as unified tools to synthesize distributed anytime computation algorithms for multi-robot motion planning which are computationally efficient and provably correct.

\emph{Contributions.} This paper presents the first distributed, anytime algorithm to compute open-loop Nash equilibrium for non-cooperative robots. More specifically, we consider a class of multi-robot motion planning problems where each robot is associated with multiple objectives and decoupled task specifications. The problems are formulated as an open-loop non-cooperative differential game. By leveraging the RRG algorithm in~\cite{SK-EF:11}, iterative better response and model checking, a distributed anytime computation algorithm, namely the iNash-trajectory algorithm, is proposed to find a Nash equilibrium of the game. We formally analyze the algorithm convergence, the price of stability as well as the computational complexity and communication cost. The algorithm performance is demonstrated by a number of numerical simulations. 

%Proofs of various theorems and lemmas are omitted due to lack of space, please refer to~\cite{Zhu.Otte.ea:arXiv-14} for the extended analysis.

%We show the following properties: (i) any limit point induced by the algorithm is a Nash equilibrium; (ii) the price of stability is equal to one for scalar cost functionals; (iii) for the worst case, the computational complexity and communication cost are shown to be linear in the robot number. It is in contrast to the exponential dependency in centralized motion planning. As the RRG algorithm, the iNash-trajectory algorithm enjoys the anytime property; i.e., quickly returning a feasible path for each robot and monotonically improving the path optimality if more computation time is allowed.

%At each iteration, each robot sequentially updates the path with a smaller cost on its current graph which fulfills the task specification and does not collide other robots.




\emph{Literature review.} Sampling based algorithms have been demonstrated to be efficient in addressing robotic motion planning in high-dimension spaces. The Rapidly-exploring Random Tree (RRT) algorithm and its variants; e.g., in~\cite{SML-JJK:00,SML-JJK:01}, are able to find a feasible path quickly. Recently, two novel algorithms, PRM$^*$ and
RRT$^*$, have been developed in~\cite{SK-EF:11}, and shown to be computationally efficient and asymptotically optimal. In~\cite{Karaman.Frazzoli:12}, a class of sampling-based algorithms is proposed to compute the optimal trajectory satisfying the given task specifications in the form of deterministic $\mu$-calculus.

%RRTs have been used for motion planning in a number of robotic systems, including autonomous cars, spacecraft and walking robots ~\cite{KKS:09,AHG:07,SLMT:11}.

Regarding the multi-robot open-loop motion planning, the approaches mainly fall into three categories: centralized planning in; e.g.,~\cite{Sanchez.Latombe:02,Xidias.Aspragathos:08}, decoupled planning in; e.g.,~\cite{Kant.Zucker:86,Simeon.Leroy.Lauumond:02} and priority planning in; e.g.,~\cite{Buckley:89,Erdmann.Lozano:87}. Centralized planning is complete but computationally expensive. In contrast, decoupled and priority planning can generate solutions quicker, but are incomplete. However, the existing algorithms assume the robots are cooperative and are not directly applicable to compute Nash equilibrium where none of self-interested robots is willing to unilaterally deviate from.

%However, none of the above results investigates game theoretic controller synthesis. In addition, priority planning imposes hierarchical ranks on the robots and is not directly applicable to compute Nash equilibrium where the robots are equal.

Another set of relevant papers are concerned with numerical methods for feedback differential games. There have been a very limited number of feedback differential games whose closed-form solutions are known, including homicidal-chauffeur and the lady-in-the-lake games ~\cite{TB-GO:99,Isaacs:99}. The methods based on partial differential
equations; e.g., in~\cite{MB-ICD:97,MB-MF-PS:99,PES:99}, viability theory; e.g.,
in~\cite{JPA:09,JPA-AB-PSP:11,PC-MQ-PSP:99} and level-set methods, e.g., in~\cite{Sethian:96} have been proposed to determine numerical solutions to differential games. However, the papers aforementioned only study one-player and two-player differential games. Multi-player linear-quadratic differential games have been studied in; e.g.,~\cite{TB-GO:99}.

%In decoupled path planning, each robot first computes its individual optimal path by ignoring other robots, and then the robots resolve the collisions along the planned paths. In centralized path planning, a centralized authority calculates the optimal paths by viewing the robot team as a composite robot.

%It is of great theoretic and practical interest to design the algorithms which are computationally efficient and simultaneously complete.

%\emph{Organization.} In Section~\ref{sec:formulation}, we will introduce the problem formulation of non-cooperative multi-robot motion planning. In Section~\ref{sec:iNash}, we will present the iNash algorithm together the analysis. In Section~\ref{sec:experiments}, a set of numerical simulations will be provided to verify the algorithm performance.

\section{Problem formulation}\label{sec:formulation}

Consider a team of robots, labeled by $\mathcal{V}_{R} \triangleq \{1,\cdots,N\}$. Each robot is associated with a dynamic system governed by the following differential equation: \begin{align}\dot{x}_i(t) = f_i(x_i(t), u_i(t)), \label{e1}\end{align} where $x_i(t)\in
\mathcal{X}_i\subseteq\real^{n_i}$ is the state of robot~$i$, and $u_i(t)\in\mathcal{U}_i$ is the control of robot~$i$. For system~\eqref{e1}, the set of admissible control strategies for robot~$i$ is given by:
\begin{align*}\mathcal{U}_i \triangleq \{u_i(\cdot) \; : \; [0,+\infty)\rightarrow U_i,\;{\rm measurable}\},\end{align*} where $U_i\subseteq\real^{m_i}$.
%
For each robot~$i$, $\sigma^{[i]} : [0,+\infty) \rightarrow \mathcal{X}_i$ is a dynamically feasible trajectory if there are $T\geq0$ and $u_i : [0,T] \rightarrow \mathcal{U}_i$ such that: $(i)$ $\dot{\sigma}^{[i]}(t) = f_i(\sigma^{[i]}(t), u_i(t))$; $(ii)$ $\sigma^{[i]}(0) = x^{[i]}_{\rm init}$; $(iii)$ $\sigma^{[i]}(t)\in \mathcal{X}_i^F$ for $t\in[0,T]$; $(iv)$ $\sigma^{[i]}(T)\in\mathcal{X}_i^G$. The set of dynamically feasible trajectories for robot~$i$ is denoted by $\Sigma_i$. Note that the trajectories in $\Sigma_i$ do not account for the inter-robot collisions.

Let $\Pi_i$ be a finite set of atomic propositions and $\lambda_i : \mathcal{X}_i\rightarrow 2^{\Pi_i}$ associates each state in $\mathcal{X}_i$ with a set of atomic propositions in $\Pi_i$. Given a trajectory $\sigma^{[i]}$ of~\eqref{e1}, define by $T(\sigma^{[i]})$ the set of time instances when $\lambda_i(\sigma^{[i]}(t))$ changes. The word $w(\sigma^{[i]}) \triangleq \{w_0, w_1, \cdots\}$ generated by the trajectory $\sigma^{[i]}$ is such that $w_i = \lambda_i(\sigma^{[i]}(t_i))$ where $t_0 = 0$ and $T(\sigma^{[i]}) = \{t_1, t_2, \cdots\}$.

In this paper, we consider reachability tasks where each robot has to reach an open goal set $\mathcal{X}_i^G \subset \mathcal{X}$ and simultaneously maintain the state $x_i(t)$ inside a closed constraint set
$\mathcal{X}_i^F \subseteq \mathcal{X}$. As an example, let $\Pi_i = \{ p_G, p_F \}$ be the set of atomic propositions. The proposition $p_G$ is true if $x_i \in \mathcal{X}_i^G$ and similarly, $p_F$ is true if $x_i \in \mathcal{X}_i^F$. Consider an example task specification $\Phi_i$ expressed using the finite fragment of Linear Temporal Logic, (FLTL)~\cite{manna1995temporal} as $\Phi_i = \mathbf{F}\ p_G \wedge \mathbf{G}\ p_F$ where $\mathbf{F}$ is the eventually operator and $\mathbf{G}$ stands for the always operator. If the word formed by a trajectory $\sigma^{[i]}$ is such that for $w(\sigma^{[i]}) = w_0, w_1, \ldots, $ there exists some $w_k $ such that $p_G \in w_k$ and $p_F \in w_i$ for all $i \geq 0$, we say that the word $w(\sigma^{[i]})$ satisfies the LTL formula $\Phi_i$. Let us note that FLTL formulae such as those considered here can be automatically translated into automata. The word $w(\sigma^{[i]})$ satisfies the formula if it belongs to the language of the corresponding automaton. Please refer~\cite{baier2008principles} for a more thorough exposition of these concepts.
%
Denote by $[\Phi_i]\subseteq \Sigma_i$ the set of trajectories fulfilling $\Phi_i$. Each robot then determines a trajectory belonging to $[\Phi_i]$.
%\pcmargin{Note that the problem formulation is general enough to incorporate full FLTL formulae, in addition to reachability and safety, see for example~\cite{kloetzer2006ltl}.}{this is not true i think}

In addition to finding a trajectory that satisfies these specifications, the robot may have several other objectives such as reaching in the goal region in the shortest possible time.
To quantify these objectives, we define ${\tt Cost} : \Sigma \rightarrow \real_{\geq0}^p$ as the cost functional which maps each trajectory in $\Sigma\triangleq\bigotimes_{i\in \mathcal{V}_R}\Sigma_i$\footnote{$\bigotimes$ represents the product.} to a non-negative cost vector and each component of ${\tt Cost}$ corresponds to an objective of the robots. In what follows, we assume that ${\tt Cost}$ is continuous. In addition, the robots want to avoid the inter-robot collisions; i.e., keeping the state $x(t)$ outside the collision set $\mathcal{X}_{\rm col}$.

The above multi-robot motion planning problem is formulated as an open-loop non-cooperative game where each robot seeks to find a trajectory which is collision-free, fulfills its task specifications and minimizes the induced cost given the trajectories of other robots. That is, given $\sigma^{[-i]}\in \Sigma_{-i}$\footnote{We use $-i$ to denote all the robots other than $i$.}, each robot~$i$ wants to find a best trajectory in the feasible set ${\tt Feasible}_i(\Sigma_i,\sigma^{[-i]})\triangleq \{\sigma^{[i]}\in\Sigma_i\;|\; \sigma^{[i]}\in[\Phi_i],\;\; {\tt CollisionFreePath}(\sigma^{[i]},\sigma^{[-i]})=1\}$ where the procedure ${\tt CollisionFreePath}$ will be defined later. The solution notion we will use is Nash equilibrium formally stated as follows:

\begin{definition}[Nash equilibrium] The collection of trajectories $(\bar{\sigma}^{[i]})_{i\in \mathcal{V}_R}\in\Sigma$ is a Nash equilibrium if for any $i\in \mathcal{V}_R$, it holds that $\bar{\sigma}^{[i]}\in {\tt Feasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ and there is no $\sigma^{[i]}\in {\tt Feasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ such that ${\tt Cost}(\sigma^{[i]})\prec{\tt Cost}(\bar{\sigma}^{[i]})$\footnote{The relation $\preceq$ is defined on $\real^p$ and given by: for $a,b\in\real^p$, $a\preceq b$ if and only if $a_{\ell} \leq b_{\ell}$ for all $\ell\in\{1,\cdots,p\}$. Note that $\preceq$ is a partial order on $\real^p$.}.\label{def2}
\end{definition}

Intuitively, none of the robots can decrease its cost by \emph{unilaterally} deviating from a Nash equilibrium. Denote by $\Pi_{\rm NE}\subseteq \Sigma$ the set of Nash equilibria. Note that Definition~\ref{def2} is an extension of the standard one; e.g., in~\cite{TB-GO:99} where the cost functional of each player is scalar. We will compare Nash equilibrium with social (Pareto) optimum defined as follows:

\begin{definition}[Social (Pareto) optimum] The collection of trajectories $(\bar{\sigma}^{[i]})_{i\in \mathcal{V}_R}\in\Sigma$ is socially (Pareto) optimal if there is no $(\sigma^{[i]})_{i\in \mathcal{V}_R}\in \Sigma$ such that $\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\sigma^{[i]}) \prec \bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\bar{\sigma}^{[i]})$.\footnote{$\bigoplus$ represents the summation.}\label{def1}
\end{definition}

Denote by $\Pi_{\rm SO}\subseteq \Sigma$ the set of social optimum. Throughout this paper, we assume that $\Pi_{\rm SO}\neq\emptyset$. In general, a Nash equilibrium may not be socially optimal. When ${\tt Cost}$ is scalar, the gap between the set of Nash equilibrium and the set of social optimum is usually characterized by price of anarchy and price of stability in; e.g.,~\cite{Nisan.Roughgarden.ea:07}.

%Although they are suboptimal, Nash equilibrium still ensures the collision avoidance and specification fulfillment. Furthermore, in non-cooperative games, each player is selfish and seeks to maximize its own interest given others' strategies. Given this fact, the process of finding a Nash equilibrium is inherently distributed and requires a much smaller amount of computation than finding a social optimum. This feature is attractive where the algorithm scalability outweighs its optimality.

%\pcmargin{The iNash-trajectory algorithm is an anytime algorithm; all anytime algorithms make the implicit assumption that an initial solution can be found quickly, relative to total planning time.}{strictly speaking, this is not the definition of anytime. I think we should neglect that particular reviewer's comments and remove this sentence}


\subsection{Primitives}

%We denote by $\phi_i(\cdot;x_i,u_i) \triangleq
%\{\phi_i(t;x_i,u_i)\}_{t\geq0}$ the solution to system~\eqref{e1} given the initial state $x_i$ and control of $u_i$. Let $\phi(\cdot;x,u)\triangleq \{\phi_i(\cdot;x_i,u_i)\}_{i\in \mathcal{V}_R}$ to be the collective solution. Define by $t_i(x,u)$ the first time when the trajectory $\phi_i(\cdot;x_i,u_i)$ hits $\mathcal{X}^G_i$ while $\phi(\cdot;x,u)$ stays in $\mathcal{X}_{\rm free}$ before $t_i(x,u)$. More precisely, given the trajectory $\phi(\cdot;x,u)$, the quantity $t_i(x,u)$ is defined as follows: \begin{align*}t_i(x,u) \triangleq &\inf\{t_i\geq0 \; | \; \phi_i(t_i;x,u)\in \mathcal{X}^G_i,\nnum\\
%&\phi(s;x,u)\in \mathcal{X}_{\rm free},\;\forall\; s\in[0,t_i]\}.\end{align*} If $\phi(\cdot;x,u)$ leaves $\mathcal{X}_{\rm free}$ before $\phi_i(\cdot;x,u)$ reaches $\mathcal{X}^G_i$ or never reaches $\mathcal{X}^G_i$, then $t_i(x,u) = +\infty$.
%
%For $a,b \in {\mathbb{R}}^N$, $a \preceq b$ if and only if $a_i \leq b_i$ for all $i$. The relation $\preceq$ defines a partial order on ${\mathbb{R}}^N$. The epigraph of a map $\phi : \mathcal{X}\rightarrow \real$, denoted by ${\rm epi}(\phi)$, is the set of $\{(x,y)\in\mathcal{X}\times\real \; | \; \phi(x) \leq y\}$. The closure of the set $A$ is denoted by ${\rm cl}(A)$. $\textbf{1}_p$ is the column vector with $p$ ones.

Here we define a set of primitives which will be used in the subsequent sections.

%\paragraph{Pareto frontier} Given a set $A$ in $\real^p$, the ${\tt Pareto}(A)$ procedure returns the minimum vectors in $A$; i.e., ${\tt Pareto}(A) = \{a\in A\;|\; \nexists b \in A \;{\rm s.t.}\; b \preceq a\}$.

\paragraph{Sampling} The ${\tt Sample}(A)$ procedure returns uniformly random samples from set $A$.

\paragraph{Local steering} Given two states $x, y$, the ${\tt Steer}$ procedure returns a state $z$ by steering $x$ towards $y$ for at most $\eta > 0$ distance; i.e., ${\tt Stear}(x,y) \triangleq {\rm argmin}_{z\in \mathbb{B}(x,\eta)}\|z-y\|$.
%
In addition to this, we require that $\sigma(x, y)$, the trajectory connecting states $x$ and $y$, is such that $|T(\sigma(x,y))| \leq 1$, i.e., the label $\lambda(\sigma(x,y))$ changes at most once. This property of the local steering function is called \emph{trace inclusivity}~\cite{castro2013incremental}.

%\paragraph{Local Steering} Given two states $x, y \in \mathcal{X}$, the ${\tt Steer}$ procedure returns a terminal time $T$ and a dynamically-feasible trajectory connecting $x$ to $y$. This trajectory is denoted by ${\tt Steer}(x,y) : [0,T] \to \mathcal{X}$.

%We assume that the local steering procedure satisfies the {\em topological property} (see~\cite{Laumond:1998wj}), i.e., for any  $\epsilon > 0$ there exists some real number $\eta_\epsilon > 0$ such that, for any two states $\State_1, \State_2 \in \StateSpace$ with $\Vert \State_1 - \State_2 \Vert < \eta$, we have $\Vert x_1 - {\tt Steer}(\State_1,\State_2) (t) \Vert < \epsilon$ for all $t \in [0,T]$. Clearly, $\eta_\epsilon \le \epsilon$. Apart from this property, we also assume $\eta_\epsilon / \epsilon$ converges to one as $\epsilon$ approaches zero.

\paragraph{Nearest neighbor} Given a state $x$ and a finite set $S$ of states, the ${\tt Nearest}$ procedure returns the state in $S$ that is closest to $x$; i.e., $\displaystyle{{\tt Nearest}(S,x) \triangleq {\rm argmin}_{y \in S}\|y-x\|}$.

\paragraph{Near vertices} Given a state $x$, a finite set $S$ and a positive real number $r$, the ${\tt NearVertices}$ procedure returns the states in $S$ where each of them is $r$-close to $x$; ${\tt NearVertices}(S, x ,r) \triangleq \{y\in S\;|\; \|x-y\|\leq r\}$.

\paragraph{Path generation} Given a directed graph $G$ with a single root and no directed cycles, the ${\tt PathGeneration}(G)$ procedure returns the set of paths from the root to the leaf vertices.

\paragraph{Collision check of paths} Given a path $\sigma$ and a set of paths $\Pi$, the ${\tt CollisionFreePath}(\sigma,\Pi)$ procedure returns $1$ if $\sigma$ collides any path in $\Pi$; i.e., $\sigma(t)\in\mathcal{X}_{\rm free} \triangleq \bigotimes_{i\in \mathcal{V}_R}\mathcal{X}^F_i \cap \overline{\mathcal{X}}_{\rm col}$; otherwise returns $0$.

%\paragraph{Path cost} Given a acyclic path $\sigma$ starting from the source, ${\tt Cost}(\sigma)$ is the cost associated with $\sigma$ if $\sigma$ reaches the destination; otherwise, ${\tt Cost}(\sigma) = +\infty$. In the paper, we assume that ${\tt Cost}$ is continuous.

%\paragraph{$\delta$-interior} A path $\sigma$ is $\delta$-interior in $\Sigma$ if $\mathbb{B}(\sigma(t),\delta)\subseteq \Sigma$ for all $t\geq0$.

\paragraph{Feasible paths} Given the path sets of $\Sigma_i$ and $\sigma^{[-i]}$, ${\tt Feasible}_i(\Sigma_i,\sigma^{[-i]})$ is the set of paths $\sigma^{[i]}\in\Sigma_i$ such that for any $\sigma^{[i]}\in\Sigma_i$, it holds that ${\tt CollisionFreePath}(\sigma^{[i]},\sigma^{[-i]}) = 1$.

\paragraph{Weakly feasible paths} Given the path sets of $\Sigma_i$ and $\sigma^{[-i]}$, ${\tt WeakFeasible}_i(\Sigma_i,\sigma^{[-i]})$ is a subset of ${\tt Feasible}_i(\Sigma_i,\sigma^{[-i]})$ and consists of the paths $\sigma^{[i]}$ where for each path $\sigma^{[i]}$, there are a sequence of paths $\{\sigma^{[i]}_{\ell}\}$ with $\sigma^{[i]}_{\ell}\in\Sigma_i$ and a diminishing and non-negative sequence $\{\delta_{\ell}\}$ such that $(i)$ $\sigma^{[i]}_{\ell}$ converges to $\sigma^{[i]}$; $(ii)$ $\mathbb{B}(\sigma^{[i]}_{\ell}(t),\delta_{\ell})\in\mathcal{X}_i^F$; $(ii)$ $\|\sigma^{[i]}_{\ell}(t)-\sigma^{[j]}(t)\|\geq \epsilon+\delta_{\ell}$ for all $j\neq i$ for all $t$.

\paragraph{Strongly feasible paths} Given the path sets of $\Sigma_i$ and $\sigma^{[-i]})$, ${\tt StrongFeasible}_i(\Sigma_i,\sigma^{[-i]})$ is a subset of ${\tt Feasible}_i(\Sigma_i,\sigma^{[-i]})$ and consists of the paths where for each path $\sigma^{[i]}$, there are a sequence of paths $\{\sigma^{[i]}_{\ell}\}$ with $\sigma^{[i]}_{\ell}\in\Sigma_i$, a diminishing and non-negative sequence $\{\delta_{\ell}\}$ and $\delta>0$ such that $(i)$ $\sigma^{[i]}_{\ell}$ converges to $\sigma^{[i]}$;  $(ii)$ $\mathbb{B}(\sigma^{[i]}_{\ell}(t),\delta_{\ell})\in\mathcal{X}_i^F$; $(ii)$ $\|\sigma^{[i]}(t)-\sigma^{[j]}(t)\|\geq \epsilon+\delta$ for all $j\neq i$.


\section{iNash-trajectory Algorithm}\label{sec:iNash}

In this section, we propose the iNash-trajectory Algorithm to solve the open-loop game defined above. It is followed by the algorithm analysis and discussion.

\subsection{Algorithm statement}

The iNash-trajectory Algorithm leverages the RRG algorithm in~\cite{SK-EF:11}, iterative better response and model checking, and informally stated as follows. At each iteration~$k$, each robot~$i$ samples $\mathcal{X}_i$ once, and adds the new sample $x_\mathrm{rand}^{[i]}$ to its vertex set $V^{[i]}_k$. Robot~$i$ extends its previously generated graph $G^{[i]}_{k-1}$ towards the new sample $x_\mathrm{rand}^{[i]}$ via local steering, and obtains the new graph $G^{[i]}_k$. After they finish the construction of the new graphs, the active robots play a game on their product graph for \emph{one} round in a \emph{sequential} way. Robot~$i$ is active at time~$k$ if its goal set is reachable through some path of $G^{[i]}_k$. The active robot with the least index, say~$i$, first chooses a smaller-cost and feasible path on $G^{[i]}_k$ by performing the ${\tt BetterResponse}$ procedure in Algorithm~\ref{algorithm:BetterResponse}. Then robot~$i$ informs all other robots the new path. After that, the active robot with the second least index, say~$j$, performs the better response to update its path on $G^{[j]}_k$ and the new path is sent to other robots. The remaining active robots sequentially update the planned paths on their own graphs and announce the new paths to others. Once all the active robots finish the path updates, the game terminates for the current iteration~$k$. At the next iteration~$k+1$, the same steps are repeated. The iNash-trajectory Algorithm is formally stated in Algorithm~\ref{algorithm:iNash-trajectory}.

The iNash-trajectory algorithm is an anytime algorithm; i.e., assuming a solution can be found within the allotted planning time, then it is continually improved while planning time remains

\subsection{Discussion}

%Here, we would like to make some remarks on the algorithm.
The ${\tt Extend}$ procedure is similar to that in the RRG algorithm~\cite{SK-EF:11} with the difference that the edges leaving from the new sample are not added. Instead, $G^{[i]}_k$ is identical to the auxiliary graph $G_n$ used in the proof of Theorem 38 in~\cite{SK-EF:11} for RRT$^*$. Notice that $G^{[i]}_k$ is a directed graph and does not include any directed circle. So there are a finite number of paths for the root to reach any leaf vertex and the ${\tt PathGeneration}$ procedure in Algorithm~\ref{algorithm:BetterResponse} is well-defined.

The tree structure returned by RRT$^*$ in~\cite{SK-EF:11} is more computationally efficient than the graph $G^{[i]}_k$ in our algorithm. However, the rewiring step in RRT$^*$ induces that $G^{[i]}_{k-1}$ may not be a subgraph of $G^{[i]}_k$. This property is crucial for the algorithm convergence to Nash equilibria in the next section.
%
To verify if $(\sigma^{[i]} \cap G_k^{[i]}) \in [\Phi_i]$ on Line 4 of the ${\tt BetterResponse}$ procedure, we check if the sequence $\xi_i = (\sigma^{[i]}\cap G^{[i]}_k)$ satisfies $\Phi_i$ by translating $\Phi_i$ into the corresponding Buchi automaton.

%\IncMargin{0.05in}

\begin{algorithm}[t] \small
  \For {$i = 1 : N$}{$V^{[i]}(0) \leftarrow x_\mathrm{init}^{[i]}$\; $E^{[i]}(0) \leftarrow \emptyset$;}
  $A_k \leftarrow \emptyset$\;
  $k\leftarrow 1$\;
  \While {$k < K$}{
  \For {$i = 1 : N$}{
    $x_\mathrm{rand}^{[i]} \leftarrow {\tt Sample}(\mathcal{X}_i)$\;
    $G^{[i]}_k\leftarrow {\tt Extend}(G^{[i]}_{k-1},x_\mathrm{rand}^{[i]})$\;}

  \For{$i\in \mathcal{V}_R\setminus A_{k-1}$}{\If{$V^{[i]}_k\cap \mathcal{X}_i^G\neq\emptyset$}{$A_k\leftarrow A_{k-1}\cup\{i\}$\;}}

  \For {$i \in A_k$}{
    $\tilde{\sigma}^{[i]}_k = \sigma^{[i]}_{k-1}$;}

  \For {$i = 1 : N$}{
  \If {$i \in A_k$}{
    $\Pi^{[i]}_k \leftarrow \{\{\sigma^{[j]}_k\}_{j\in A_k, j<i},\{\tilde{\sigma}^{[j]}_k\}_{j\in A_k, j>i}\}$\;
    $\sigma^{[i]}_k \leftarrow {\tt BetterResponse}(G^{[i]}_k, \Pi^{[i]}_k)$;}}
    $k \leftarrow k + 1$;
  }
  \caption{The iNash-trajectory Algorithm}
  \label{algorithm:iNash-trajectory}
\end{algorithm}

\begin{algorithm}[t] \small
  $V \leftarrow V^{[i]}_{k-1}$\;
  $E \leftarrow E^{[i]}_{k-1}$\;
  $x_\mathrm{nearest} \leftarrow {\tt Nearest} (E, x_\mathrm{rand}^{[i]})$\;
  $x_\mathrm{new} \leftarrow {\tt Steer} (x_\mathrm{nearest}, x_\mathrm{rand}^{[i]})$\;
  \If {${\tt ObstacleFree} (x_\mathrm{nearest},x_\mathrm{new})$} {
  $\mathcal{X}_\mathrm{near}\leftarrow {\tt NearVertices}(E,x_\mathrm{new},\min\{\gamma(\frac{\log k}{k})^{\frac{1}{n}},\eta\})$\;
  $V\leftarrow V\cup\{x_\mathrm{new}\}$\;
  \For {$x_\mathrm{near}\in \mathcal{X}_\mathrm{near}$}{ \If{${\tt ObstacleFree}(x_\mathrm{nearest},x_\mathrm{new})$}{$E\leftarrow E\cup\{(x_\mathrm{nearest},x_\mathrm{new})\}$\;}}}
  \Return{$G = (V, E)$}
  \caption{The ${\tt Extend}$ Procedure}
  \label{algorithm:extend-mRRG}
\end{algorithm}

\begin{algorithm}[t] \small
  $\mathbb{P}^{[i]}_k \leftarrow {\tt PathGeneration}(G^{[i]}_k)$\;
  $\mathbb{P}^{[i]}_{\rm f} \leftarrow \emptyset$\;
  \For{$\sigma^{[i]}\in \mathbb{P}^{[i]}_k$}{\If{${\tt CollisionFreePath}(\sigma^{[i]},\Pi^{[i]}_k) == 1\;\&\& \; (\sigma^{[i]}\cap G^{[i]}_k)\in [\Phi_i]$}{$\mathbb{P}^{[i]}_{\rm f}\leftarrow \mathbb{P}^{[i]}_{\rm f}\cup\{\sigma^{[i]}\}$\;}}
  $\sigma^{[i]}_{\rm min} \leftarrow \sigma^{[i]}_{k-1}$\;
  \For{$\sigma^{[i]} \in \mathbb{P}^{[i]}_{\rm f}$}{\If{${\tt Cost}(\sigma^{[i]}) \prec {\tt Cost}(\sigma^{[i]}_{\rm min})$}{$\sigma^{[i]}_{\rm min}\leftarrow \sigma^{[i]}$\;
  Break\;}}
  \Return{$\sigma^{[i]}_{\rm min}$}
  \caption{The ${\tt BetterResponse}$ Procedure}
  \label{algorithm:BetterResponse}
\end{algorithm}

%\begin{algorithm}[h] \small
%  $\mathbb{P}^{[i]}_k \leftarrow {\tt PathGeneration}(G^{[i]}_k)$\;
%  $\mathbb{P}^{[i]}_{\rm cf} \leftarrow \emptyset$\;
%  \For{$\sigma^{[i]}\in \mathbb{P}^{[i]}_k$}{\If{${\tt CollisionFreePath}(\sigma^{[i]},\Pi^{[i]}_k) == 1$}{$\mathbb{P}^{[i]}_{\rm cf}\leftarrow \mathbb{P}^{[i]}_{\rm cf}\cup\{\sigma^{[i]}\}$\;}}
%  $\sigma^{[i]}_{\rm min} \leftarrow {\tt Sample}(\mathbb{P}^{[i]}_{\rm cf})$\;
%  \For{$\sigma^{[i]} \in \mathbb{P}^{[i]}_{\rm cf}$}{\If{${\tt Cost}(\sigma^{[i]}) < {\tt Cost}(\sigma^{[i]}_{\rm min})$}{$\sigma^{[i]}_{\rm min}\leftarrow \sigma^{[i]}$\;}}
%  \Return{$\sigma^{[i]}_{\rm min}$}
%  \caption{The ${\tt BestResponse}$ Procedure}
%  \label{algorithm:BestResponse}
%\end{algorithm}


\subsection{Analysis}

In this section, we analyze the asymptotic optimality, computational complexity, and communication cost of the iNash-trajectory Algorithm. Before doing that, we first prove the existence of Nash equilibrium.

\begin{lemma}[Existence of Nash equilibrium] It holds that $\Pi_{\rm SO} \subseteq \Pi_{\rm NE}$ and $\Pi_{\rm NE}$ is non-empty.\label{lem1}
\end{lemma}

\begin{proof} We will show $\Pi_{\rm SO} \subseteq \Pi_{\rm NE}$ by contradiction. Assume $\Pi_{\rm SO} \cap \overline{\Pi}_{\rm NE} \neq \emptyset$ with $\overline{\Pi}_{\rm NE}$ being the complement of $\Pi_{\rm NE}$, and pick any $(\sigma^{[i]})_{i\in \mathcal{V}_R}\in\Pi_{\rm SO} \cap \overline{\Pi}_{\rm NE}$. Since $(\sigma^{[i]})_{i\in \mathcal{V}_R}\notin\Pi_{\rm NE}$, there is some $i\in \mathcal{V}_R$ and $\bar{\sigma}^{[i]}\in {\tt Feasible}_i(\Sigma_i,\sigma^{[-i]})$ such that ${\tt Cost}(\bar{\sigma}^{[i]}) \prec {\tt Cost}(\sigma^{[i]})$.

Let $(\bar{\sigma}^{[i]})_{i\in \mathcal{V}_R}$ with $\bar{\sigma}^{[j]} = \sigma^{[j]}$ for $j\neq i$. Note that $\bar{\sigma}^{[j]}\in {\tt Feasible}_j(\Sigma_j,\bar{\sigma}^{[-j]})$ for all $j\neq i$, and ${\tt Cost}(\bar{\sigma}^{[i]}) = {\tt Cost}(\sigma^{[i]})$. It implies that $\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\bar{\sigma}^{[i]}) \prec \bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\sigma^{[i]})$ contradicting that $(\sigma^{[i]})_{i\in \mathcal{V}_R}$ is socially optimal. Therefore, $\Pi_{\rm SO} \subseteq \Pi_{\rm NE}$. Since $\Pi_{\rm SO}$ is non-empty, so is $\Pi_{\rm NE}$.
\end{proof}

In what follows, we establishes the following two instrumental results for asymptotic optimality. In particular, Lemma~\ref{lem3} shows that given $\sigma^{[-i]}$, any path in ${\tt WeakFeasible}_i(\Sigma_i,\sigma^{[-i]})$ can be approximated by a sequence of paths in $\{{\tt Feasible}_i(G^{[i]}_k,\sigma^{[-i]})\}_{k\geq0}$. Lemma~\ref{lem4} shows that ${\tt Strong Feasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ is a subset of $\sigma^{[i]}\in {\tt Strong Feasible}_i(\Sigma_i,\sigma^{[-i]}_k)$ for all but a finite $k$ where $\{\sigma^{[-i]}_k\}_{k\geq0}$ converges to $\bar{\sigma}^{[-i]}$.

\begin{lemma} Consider any $i\in \mathcal{V}_R$ and $\sigma^{[-i]}\in \bigotimes_{j\neq i}\Sigma_j$. For any $\varepsilon>0$, there is $T\geq0$ such that for any $k\geq T$ and $\sigma^{[i]}\in {\tt WeakFeasible}_i(\Sigma_i,\sigma^{[-i]})$, there is a path $\hat{\sigma}^{[i]}_k\in {\tt Feasible}_i(G^{[i]}_k,\sigma^{[-i]})$ such that $\|{\tt Cost}(\hat{\sigma}^{[i]}_k) - {\tt Cost}(\sigma^{[i]})\|\leq\varepsilon$.\label{lem3}\end{lemma}

\begin{proof} Recall that $G^{[i]}_k$ is identical to the auxiliary graph $G_n$ used in the proof of Theorem 38 for RRT$^*$. One can finish the proof by following the lines for Theorem 38 in~\cite{SK-EF:11} and using the fact of ${\tt Cost}$ being continuous.
\end{proof}


\begin{lemma} Assume that $\{\sigma^{[j]}_k\}_{k\geq0}$ converges to $\bar{\sigma}^{[j]}$ for each $j\in \mathcal{V}_R$. There is $T'\geq0$ such that for each $\sigma^{[i]}\in {\tt Strong Feasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$, it holds that $\sigma^{[i]}\in {\tt Strong Feasible}_i(\Sigma_i,\sigma^{[-i]}_k)$ for all $k\geq T'$.\label{lem4}\end{lemma}

\begin{proof} Since $\{\sigma^{[j]}_k\}_{k\geq0}$ converges to $\bar{\sigma}^{[j]}$, there is $T'\geq0$ such that $\|\bar{\sigma}^{[j]}(t)-\sigma^{[j]}_k(t)\|\leq \frac{\delta}{2}$ for all $k\geq T'$ and $j\in \mathcal{V}_R$.

Recall that $\sigma^{[i]}\in {\tt Strong Feasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$. Then there are $\{\sigma^{[i]}_{\ell}\}$ converging to $\sigma^{[i]}$, $\{\delta_{\ell}\}$ and $\delta > 0$ such that $\mathbb{B}(\sigma^{[i]}_{\ell}(t),\delta_{\ell})\in \mathcal{X}_i^F$ and $\|\sigma^{[i]}(t)-\bar{\sigma}^{[j]}(t)\|\geq \epsilon+\delta$. This further implies that $\|\sigma^{[i]}(t)-\sigma^{[j]}_k(t)\|\geq \epsilon+\frac{\delta}{2}$ for all $k\geq T'$. We then conclude that $\sigma^{[i]}\in {\tt Strong Feasible}_i(\Sigma_i,\sigma^{[-i]}_k)$ for all $k\geq T'$.
\end{proof}

Let $\hat{\Sigma}$ be the set of limit points of $\{\sigma^{[i]}_k\}_{i\in \mathcal{V}_R}$. We are ready to show the convergence of the iNash-trajectory Algorithm.

\begin{theorem}[Asymptotic optimality] There is $\bar{c}^{[i]}_{\ell}\geq0$ such that ${\tt Cost}_{\ell}(\bar{\sigma}^{[i]}) = \bar{c}^{[i]}_{\ell}$ for any $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}\in\hat{\Sigma}$. In addition, any limit point $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}$ is a Nash equilibrium.\label{the2}
\end{theorem}

\begin{proof} First of all, notice that $A_k \subseteq A_{k+1}$. It renders that the probability of $A_k \neq \mathcal{V}_R$ exponentially decays to zero. Without loss of any generality, we consider $A_k = \mathcal{V}_R$ for $k\geq0$ in the remainder of the proof.

Consider iteration $k$ and assume that robots $1,\cdots,i-1$ have updated their paths via Step 16 in the iNash-trajectory Algorithm. It is the turn of robot~$i$ to update the path via Step 16. Since $G^{[i]}_{k}\supseteq G^{[i]}_{k-1}$, so $\tilde{\sigma}^{[i]}_k = \sigma^{[i]}_{k-1}\in {\tt Feasible}_i(G^{[i]}_k,\Pi^{[i]}_k)$. This implies \begin{align*}{\tt Cost}(\sigma^{[i]}_k) \preceq {\tt Cost}(\tilde{\sigma}^{[i]}_k) = {\tt Cost}(\sigma^{[i]}_{k-1}).\end{align*} Hence, for each $\ell\in\{1,\cdots,p\}$, the sequence $\{{\tt Cost}_{\ell}(\sigma^{[i]}_k)\}_{k\geq0}$ is non-increasing. Since ${\tt Cost}_{\ell}(\sigma^{[i]}_k)$ is lower bounded by zero, so the sequence $\{{\tt Cost}_{\ell}(\sigma^{[i]}_k)\}$ converges to some constant $\bar{c}^{[i]}_{\ell}\geq0$. Hence, we have ${\tt Cost}_{\ell}(\bar{\sigma}^{[i]}) = \bar{c}^{[i]}_{\ell}$ for any limit point $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}\in\hat{\Sigma}$.

We proceed to show that any limit point $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}\in\hat{\Sigma}$ is a Nash equilibrium. There is a subsequence $\pi_i\triangleq\{k_i\}$ such that $\{\sigma_k^{[i]}\}_{k\in \pi_i}$ converge to $\bar{\sigma}^{[i]}$. Let $\pi \triangleq \cap_{i\in \mathcal{V}_R}\pi_i$. Without loss of any generality, we assume that $\pi = \mathbb{Z}_{\geq0}$ in the remainder of the proof.

For each $i\in \mathcal{V}_R$, there is no $\tilde{\sigma}^{[i]}_k\in {\tt Feasible}_i(G^{[i]}_k,\Pi^{[i]}_k)$ such that \begin{align}{\tt Cost}(\sigma^{[i]}_k) \succeq {\tt Cost}(\tilde{\sigma}^{[i]}_k).\label{e5}\end{align}

Pick any $\varepsilon>0$. By Lemma~\ref{lem3} with $\sigma^{[-i]} = \Pi^{[i]}_k$, there is $T$ such that for any $\hat{\sigma}^{[i]}_k\in {\tt WeakFeasible}_i(\Sigma_i,\Pi^{[i]}_k)$, there is $\tilde{\sigma}^{[i]}_k\in {\tt Feasible}_i(G^{[i]}_k,\Pi^{[i]}_k)$ such that \begin{align}{\tt Cost}(\tilde{\sigma}^{[i]}_k) \preceq {\tt Cost}(\hat{\sigma}^{[i]}_k)+\varepsilon \textbf{1}_p,\label{e6}\end{align} where $\textbf{1}_p$ is the column vector with $p$ ones.

The combination of~\eqref{e5} and~\eqref{e6} renders that there is no $\hat{\sigma}^{[i]}_k\in {\tt WeakFeasible}_i(\Sigma_i,\Pi^{[i]}_k)$ and $k\geq T$ such that: \begin{align}{\tt Cost}(\sigma^{[i]}_k)\succeq{\tt Cost}(\hat{\sigma}^{[i]}_k)+\varepsilon\textbf{1}_p.\label{e7}\end{align}

Notice that ${\tt StrongFeasible}_i(\Sigma_i,\Pi^{[i]}_k)\subseteq{\tt WeakFeasible}_i(\Sigma_i,\Pi^{[i]}_k)\subseteq {\tt Feasible}_i(\Sigma_i,\Pi^{[i]}_k)$. Combine this property,~\eqref{e6} and Lemma~\ref{lem4} with $\sigma^{[i]}_k = \Pi^{[i]}_k$, and we reach that there is $T'\geq T$ such that there is no $\sigma^{[i]}\in {\tt StrongFeasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ and $k\geq T'$ such that: \begin{align}{\tt Cost}(\sigma^{[i]}_k)\succeq {\tt Cost}(\sigma^{[i]})+\varepsilon\textbf{1}_p.\label{e2}\end{align}

Take the limit on $k$ in~\eqref{e2}. Then there is no ${\sigma}^{[i]}\in {\tt StrongFeasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ such that \begin{align}{\tt Cost}(\bar{\sigma}^{[i]})\succeq {\tt Cost}({\sigma}^{[i]})+\varepsilon\textbf{1}_p.\label{e3}\end{align} Since~\eqref{e3} holds for $\varepsilon>0$, no ${\sigma}^{[i]}\in {\tt StrongFeasible}_i(\Sigma_i,\bar{\sigma}^{[-i]})$ exists such that \begin{align}{\tt Cost}(\bar{\sigma}^{[i]})\succeq {\tt Cost}({\sigma}^{[i]}).\label{e4}\end{align} Since~\eqref{e4} holds for any~$i\in \mathcal{V}_R$, it establishes that the limit point $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}$ is a Nash equilibrium.

The completeness of the algorithm is a direct result of Lemma~\ref{lem1} and the asymptotic convergence to $\Pi_{\rm NE}$. It completes the proof.
\end{proof}

Next, we will analyze the computational complexity of the algorithm in terms of the ${\tt CollisionFreePath}$ procedure. Let $\theta_n$ to be the total number of calls to the ${\tt CollisionFreePath}$ procedure at iteration~$n$.

\begin{lemma}[Computational complexity] It holds that $\theta_n \leq \bigoplus_{i\in \mathcal{V}_R}|\mathbb{P}^{[i]}_k|$, where $\mathbb{P}^{[i]}_k$ is defined in Algorithm~\ref{algorithm:BetterResponse}.\label{lem2}
\end{lemma}

In Lemma~\ref{lem2}, the quantity $|\mathbb{P}^{[i]}_k|$ is independent of the robot number. So the worst computational complexity of the iNash-trajectory grows linearly in the robot number. It is in contrast to the exponential dependency in centralized path planning. The computational efficiency comes with the non-cooperative game theoretic formulation where each robot myopically responds to others. Note that a Nash equilibrium may not be socially optimal for the robot team.

Let $\vartheta_n$ to be the number of exchanged paths in iteration~$n$. The following lemma shows the worst communication cost is linear in the robot number.

\begin{lemma}[Communication cost] $\vartheta_n \leq 2N$.\label{lem5}
\end{lemma}

\begin{proof} At iteration~$n$, all the active robots broadcast their planned trajectories before the path updates. Then the active robots sequentially update and broadcast the new paths. So $\vartheta_n = 2|A_k| \leq 2N$.
\end{proof}

\subsection{Comparison}

%\begin{table}[ht]
%\caption{The comparison for the feedback case}
%\centering
%\begin{tabular}{|c|c|c|}
%  \hline
%  & \text{Optimal control} & \text{Cooperative game}
%\tabularnewline
%  \hline
%  \text{Solution Optimality}  & \text{High} & \text{Medium}
%\tabularnewline
%  \hline
%  \text{Coordination}  & \text{High} & \text{Medium}
%\tabularnewline
%  \hline
%  \text{Asymptotic optimality}  & \text{N/A} & \text{Yes}
%\tabularnewline
%  \hline
%  \text{Computational complexity}  & \text{Exp.} & \text{From linear to exp.}
%\tabularnewline
%  \hline
%\end{tabular}\label{ta:com1}
%\end{table}

In order to demonstrate the scalability of iNash-trajectory Algorithm, we consider the benchmark algorithm, the iOptimalControl Algorithm. The key difference between the iOptimalControl and iNash-trajectory Algorithms is that a centralized authority in the iOptimalControl Algorithm determines a social optimum on the product graph at each iteration. In the iOptimalControl Algorithm, we use the notation $(\sigma\cap \bigotimes_{i\in A_k}G^{[i]}_k)\in[\Phi]$ for $(\sigma^{[i]}\cap G^{[i]}_k)\in[\Phi_i]$ for all $i\in A_k$.

%\IncMargin{0.05in}

\begin{algorithm}[t] \small
  \For {$i = 1 : N$}{$V^{[i]}(0) \leftarrow x_\mathrm{init}^{[i]}$\; $E^{[i]}(0) \leftarrow \emptyset$;}
  $A_k \leftarrow \emptyset$\;
  $k\leftarrow 1$\;
  \While {$k < K$}{
  \For {$i = 1 : N$}{
    $x_\mathrm{rand}^{[i]} \leftarrow {\tt Sample}(\mathcal{X}_i)$\;
    $G^{[i]}_k\leftarrow {\tt Extend}(G^{[i]}_{k-1},x_\mathrm{rand}^{[i]})$\;}

  \For{$i\in \mathcal{V}_R\setminus A_{k-1}$}{\If{$V^{[i]}_k\cap \mathcal{X}_i^G\neq\emptyset$}{$A_k\leftarrow A_{k-1}\cup\{i\}$\;}}

  $(\sigma^{[i]}_k)_{i\in A_k}\leftarrow{\tt OptimalTrajectory}(\bigotimes_{i\in A_k}G^{[i]}_k)$\;

  $k \leftarrow k + 1$;
  }
  \caption{The iOptimalControl Algorithm}
  \label{algorithm:iOptimalControl}
\end{algorithm}

\begin{algorithm}[t] \small
  \For{$i\in A_k$}{$\mathbb{Q}^{[i]}_k \leftarrow {\tt PathGeneration}(G^{[i]}_k)$\;}

  \For{$i\in A_k$}{
  $\mathbb{P}^{[i]}_{\rm f} \leftarrow \emptyset$\;
  \For{$\sigma^{[i]}\in \mathbb{Q}^{[i]}_k$}{\If{${\tt CollisionFreePath}(\sigma^{[i]},\mathbb{Q}^{[-i]}_k) == 1\;\&\&\; (\sigma\cap \bigotimes_{i\in A_k}G^{[i]}_k)\in[\Phi]$}{$\mathbb{P}^{[i]}_{\rm f}\leftarrow \mathbb{P}^{[i]}_{\rm f}\cup\{\sigma^{[i]}\}$\;}}}

  $\sigma_{\rm min} \leftarrow {\tt Sample}(\bigotimes_{i\in A_k}\mathbb{P}^{[i]}_{\rm f})$\;

  \For{$\sigma \in \bigotimes_{i\in A_k}\mathbb{P}^{[i]}_{\rm f}$}{\If{$\bigoplus_{i\in A_k}{\tt Cost}(\sigma^{[i]}) \prec \bigoplus_{i\in A_k}{\tt Cost}(\sigma^{[i]}_{\rm min})$}{$\sigma_{\rm min}\leftarrow \sigma$\;}}
  \Return{$\sigma_{\rm min}$}
  \caption{The ${\tt OptimalTrajectory}$ Procedure}
  \label{algorithm:OptimalTrajectory}
\end{algorithm}

The following theorem guarantees the asymptotic optimality of the iOptimalControl Algorithm.

\begin{theorem}[Asymptotic optimality] Any limit point $\{\bar{\sigma}^{[i]}\}_{i\in \mathcal{V}_R}$ is a social optimum.\label{the3}
\end{theorem}

\begin{proof} It is a direct result of Theorem 38 in~\cite{SK-EF:11}.
\end{proof}

Next, we will analyze the computational complexity of the algorithm in terms of the ${\tt CollisionFreePath}$ procedure. Let $\theta_n'$ to be the total number of calls to the ${\tt CollisionFreePath}$ procedure at iteration~$n$.

\begin{lemma}[Computational complexity] It holds that $\theta'_n = \bigotimes_{i\in \mathcal{V}_R}|\mathbb{Q}^{[i]}_k|$, where $\mathbb{Q}^{[i]}_k$ is defined in Algorithm~\ref{algorithm:OptimalTrajectory}.\label{lem6}
\end{lemma}

The above lemma shows that the computational complexity exponentially grows vs.\ robot number. Table~\ref{ta:com2} summarizes the comparison of the iOptimalControl and iNash-trajectory Algorithms where the prices of anarchy and stability are compared for the case $p = 1$. In particular, the price of stability (POS)~\cite{Nisan.Roughgarden.ea:07} is the ratio between the minimum additive cost function value in $\Pi_{\rm NE}$ and that of one in $\Pi_{\rm SO}$, and defined as follows:
\begin{align*}{\rm POS} = \frac{\inf_{\sigma\in\Pi_{\rm NE}}\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\sigma^{[i]})}{\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\bar{\sigma}^{[i]})},\end{align*} for any $\bar{\sigma}\in\Pi_{\rm SO}$. By Lemma~\ref{lem1}, we know $\Pi_{\rm SO}\subseteq \Pi_{\rm NE}$ and thus POS is equal to 1. On the other hand, the price of anarchy (POA)~\cite{Nisan.Roughgarden.ea:07} is the ratio between the maximum additive cost function value in $\Pi_{\rm NE}$ and that of one in $\Pi_{\rm SO}$, and given by: \begin{align*}{\rm POA} = \frac{\sup_{\sigma\in\Pi_{\rm NE}}\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\sigma^{[i]})}{\bigoplus_{i\in \mathcal{V}_R}{\tt Cost}(\bar{\sigma}^{[i]})},\end{align*} for any $\bar{\sigma}\in\Pi_{\rm SO}$. The value of POA depends on a number of factors; e.g., the obstacle locations, the dynamic systems and so on. It is interesting to find a lower bound on $\Pi_{\rm SO}$ given more information as in; e.g.,~\cite{Johari.Tsitsiklis:04}, and utilize mechanism design to eliminate the price of anarchy.

\begin{table}[ht]
\caption{The comparison of the iOptimalControl and iNash-trajectory Algorithms}
\centering
\begin{tabular}{|c|c|c|}
  \hline
  & \text{iOptimalControl} & \text{iNash-trajectory}
\tabularnewline
  \hline
  \text{Solution Notion}  & \text{Social optimum} & \text{Nash equilibrium}
\tabularnewline
  \hline
  \text{Solution Feasibility}  & \text{Yes} & \text{Yes}
\tabularnewline
  \hline
  \text{Price of stability}  & \text{N/A} & \text{One}
\tabularnewline
  \hline
  \text{Price of anarchy}  & \text{N/A} & \text{Unknown}
\tabularnewline
  \hline
  \text{Coordination}  & \text{High} & \text{Low}
\tabularnewline
  \hline
  \text{Asymptotic optimality}  & \text{Yes} & \text{Yes}
\tabularnewline
  \hline
  \text{Computational complexity}  & \text{Exponential} & \text{Linear}
\tabularnewline
  \hline
\end{tabular}\label{ta:com2}
\end{table}


\section{Experiments}\label{sec:experiments}

We perform two experiments in simulation to evaluate the performance of iNash. The first involves 8 circular robots moving in an environment with randomly generated obstacles (Figure~\ref{fig:exp1}-left), while the second involves 6 robots in a traffic intersection scenario (Figure~\ref{fig:exp1}-right); both involve state spaces consisting of first order dynamics and time. Robots are holonomic discs with radii of $0.5$ meters. %The third is similar to the first, except that it considers 5 robots with second order dynamics, while the objective function is time (Figure~\ref{fig:exp3}).

We compare iNash to two prioritized methods that are not guaranteed to return a Nash-Equilibrium, but are arguably similar to our proposed algorithm. The first is the standard prioritized approach from \cite{Erdmann.Lozano:87}. Each robot builds its own random graph; then robots select their paths in order such that the path of robot $i$ does not conflict with robots ${1, \ldots, i-1}$.
%
The second is an any-time version of the prioritized method. Each time robot $i$ finds a better path that does not conflict with the paths of robots $1, \ldots, i-1$, then for $j = i+1, i+2, \ldots$ (in order) robot $j$ must choose a new path that does not conflict with robots $1, \ldots, j-1$. This differs from our algorithm (where new paths must respect the paths of all other robots), and the solution is not guaranteed to converge to a Nash Equilibrium.

For experiments we consider the task specifications for each robot to be of the form
$\Phi_i = \mathbf{F}\ p_{G_i} \wedge \mathbf{G}\ p_F$, i.e., each robot tries to reach a different goal region in the shortest possible distance
% (first and second experiments) or time (third experiment)
while respecting the same constraint set $\mathcal{X}^F$. The automaton consists of two states (see Fig.~\ref{fig:buchi}).

\begin{figure}[!t]
\centering
\usetikzlibrary{arrows,positioning,automata,shadows,fit,shapes}
\begin{tikzpicture}[->,>=stealth',shorten >=1pt,auto,node distance=1.5cm, semithick, initial text=]
\tikzset{every state/.style={minimum size=1pt}}

\node[state, initial] 			at (0,0)		(q0) {$q_0$};
\node[state, accepting]      	at (2.5, 0)   	(q1) {$q_1$};

\begin{scope}[every node/.style={scale=.8}]
	\path 	(q0) edge [loop above] node {$p_F \wedge \neg p_{G_i}$} (q0)
				edge node {$p_F \wedge p_{G_i}$} (q1)
			(q1) edge [loop above] node {$p_F$} (q1);
\end{scope}
\end{tikzpicture}
\caption{\small Automaton for $\Phi_i = \mathbf{F}\ p_{G_i} \wedge \mathbf{G}\ p_F$.}
\label{fig:buchi}
\end{figure}


\begin{figure*}[t]
    \centering
 \includegraphics[width=0.3\linewidth, trim=155 230 155 230, clip=true]{figures/6_of_8}
 \hspace{0.2in}
 \includegraphics[width=0.3\linewidth, trim=155 230 155 230, clip=true]{figures/8_of_8}
 \hspace{0.2in}
 \includegraphics[width=0.3\linewidth, trim=155 230 155 230, clip=true]{figures/6_of_6}


   \caption[]{\small Experimental environments. Obstacles are outlined in black, paths are colored lines. Robots start at `O's and end at `X's ('O's are drawn 3x the robot radii to help visualization). {\bf Left/Center:} 8 robots in a randomly generated environment; the Nash Equilibrium in the left trial allows 6 of 8 robots to reach their goals, while all 8 reach their goals in the center trial. {\bf Right:} 6 Robots at an intersection and the paths corresponding to a Nash equilibrium where all robots reach their goals}


\label{fig:exp1}
\end{figure*}



%\begin{figure}[t]
%
%  \footnotesize
%
% %\includegraphics[width=\linewidth, trim=100 230 100 230, clip=true]{figures/3_of_6}
% \includegraphics[width=\linewidth, trim=100 230 100 230, clip=true]{figures/6_of_6}
%
%
%   \caption[]{6 Robots in an intersection scenario and the paths corresponding to a Nash equilibrium. Obstacles are outlined in black, robot paths are colored lines; Paths start at `O's and end at `X's. 'O's are drawn 3x the robot radius to make visualization easier.}
%\label{fig:exp2}
%\end{figure}




%\begin{figure}[t]
%
%  \footnotesize
%
% %\includegraphics[width=\linewidth, trim=100 230 100 230, clip=true]{figures/4_of_5}
% \includegraphics[width=\linewidth, trim=100 230 100 230, clip=true]{figures/5_of_5}
%
%   \caption[]{5 robots using second order dynamics in a randomly generated environment and the paths corresponding to a Nash equilibrium. Obstacles are outlined in black, paths are colored lines; Paths start at `O's and end at `X's. Note that the 'O's are drawn 3x the robot radius to make visualization easier.}
%\label{fig:exp3}
%\end{figure}

\newlength{\mycolw}
\setlength{\mycolw}{.69cm}

\begin{table*}[!t]

  \caption[Results]{Experimental Results, Random Environment}
  \label{table:A}

\begin{minipage}[b]{0.67\linewidth}

  \renewcommand{\arraystretch}{1.2}
  \begin{tabular}{ | l | x{\mycolw} | x{\mycolw} | x{\mycolw} | x{\mycolw} | x{\mycolw} | x{\mycolw} | x{\mycolw} | x{\mycolw} |}
     \multicolumn{9}{l}{\hspace{2.7cm} Mean path length over 20 trials (ratio vs.\ socially optimal length)}\tabularnewline
  \hline
     \multirow{2}{*}{Algorithm} & \multicolumn{8}{|c|}{ Robot ID}\tabularnewline
  \cline{2-9}
      & 1 & 2 & 3 & 4 & 5 & 6 & 7 & 8\tabularnewline
  \cline{1-9}
      iNash (any-time)      & $1.295$ & $1.343$ & $1.324$ & $1.301$ & $1.166$ & $1.293$ & $1.224$ & $1.202$\tabularnewline
  \cline{1-9}
      Prioritized & $1.084$ & $1.149$ & $1.263$ & $1.316$ & $1.228$ & $1.326$ & $1.312$ & $1.349$\tabularnewline
  \cline{1-9}
      Prioritized (any-time)  & $1.081$ & $1.129$ & $1.153$ & $1.200$ & $1.126$ & $1.168$ & $1.163$ & $1.204$\tabularnewline
  \hline
  \end{tabular}


\end{minipage}
\begin{minipage}[b]{0.3\linewidth}

  \renewcommand{\arraystretch}{1.2}
  \begin{tabular}{ | c | c | c | c | c | c | c | c |}
     \multicolumn{8}{l}{Total times reached goal (of 20)}\\
  \hline
      \multicolumn{8}{|c|}{ Robot ID}\\
  \cline{1-8}
       1 & 2 & 3 & 4 & 5 & 6 & 7 & 8\\
  \cline{1-8}
      20 & 19 & 18 & 20 & 16 & 15 & 18 & 17\\
  \cline{1-8}
      20 & 20 & 18 & 19 & 20 & 18 & 18 & 15\\
  \cline{1-8}
      20 & 20 & 18 & 20 & 19 & 20 & 20 & 20\\
  \hline
  \end{tabular}

\end{minipage}

\end{table*}

\newlength{\mycolwd}
\setlength{\mycolwd}{.9cm}

\begin{table*}[!t]

  \caption[Results]{Experimental Results, Intersection Environment}
  \label{table:B}

\begin{minipage}[b]{0.67\linewidth}

  \renewcommand{\arraystretch}{1.2}
  \begin{tabular}{ | l | x{\mycolwd} | x{\mycolwd} | x{\mycolwd} | x{\mycolwd} | x{\mycolwd} | x{\mycolwd} |}
     \multicolumn{7}{l}{\hspace{2.7cm} Mean path length over 20 trials (ratio vs.\ socially optimal length)}\tabularnewline
  \hline
     \multirow{2}{*}{Algorithm} & \multicolumn{6}{|c|}{ Robot ID}\tabularnewline
  \cline{2-7}
      & 1 & 2 & 3 & 4 & 5 & 6\tabularnewline
  \cline{1-7}
      iNash (any-time) & $1.168$ & $1.228$ & $1.245$ & $1.243$ & $1.201$ & $1.166$\tabularnewline
  \cline{1-7}
      Prioritized & $1.107$ & $1.238$ & $1.384$ & $1.492$ & $1.339$ & $1.300$\tabularnewline
  \cline{1-7}
      Prioritized (any-time)  & $1.085$ & $1.212$ & $1.165$ & $1.248$ & $1.136$ & $1.251$\tabularnewline
  \hline
  \end{tabular}




\end{minipage}
\begin{minipage}[b]{0.3\linewidth}

  \renewcommand{\arraystretch}{1.2}
  \begin{tabular}{  | c | c | c | c | c | c |}
     \multicolumn{6}{l}{Total times reached goal (of 20)}\\
  \hline
     \multicolumn{6}{|c|}{ Robot ID}\\
  \cline{1-6}
       1 & 2 & 3 & 4 & 5 & 6\\
  \cline{1-6}
       11 & 14 & 12 & 14 & 16 & 15\\
  \cline{1-6}
       19 & 18 & 19 & 13 & 12 & 4\\
  \cline{1-6}
       19 & 18 & 20 & 19 & 20 & 15\\
  \hline
  \end{tabular}

\end{minipage}

\end{table*}



%\begin{table*}[!t]
%
%  \centering
%
%  \caption[Results]{Experimental Results, Second Order Dynamics Experiment}
%  \label{table:C}
%
%  %\renewcommand{\arraystretch}{1.7}
%  \begin{tabular}{ | c | c | c | c | c | c |}
%     \multicolumn{6}{l}{Mean path time over 20 trials (s)}\\
%  \hline
%     \multirow{2}{*}{Algorithm} & \multicolumn{5}{|c|}{ Robot ID}\\
%  \cline{2-6}
%      & 1 & 2 & 3 & 4 & 5\\
%  \cline{1-6}
%      iNash (any-time) & $84.41$ & $73.53$ & $73.11$ & $77.20$ & $91.33$\\
%  \cline{1-6}
%      Prioritized Planning (any-time) & $59.56$ & $64.25$ & $56.25$ & $79.42$ & $77.79$ \\
%  \cline{1-6}
%      Prioritized Planning (offline)  & $53.47$ & $59.60$ & $64.88$ & $78.53$ & $80.12$ \\
%  \hline
%  \end{tabular}
%
%\vspace{0.3cm}
%
%  %\renewcommand{\arraystretch}{1.7}
%  \begin{tabular}{ | c | c | c | c | c | c |}
%     \multicolumn{6}{l}{Total times successfully reached goal out of 20}\\
%  \hline
%     \multirow{2}{*}{Algorithm} & \multicolumn{5}{|c|}{ Robot ID}\\
%  \cline{2-6}
%      & 1 & 2 & 3 & 4 & 5\\
%  \cline{1-6}
%      iNash (any-time) & 15 & 12 & 11 & 18 & 11\\
%  \cline{1-6}
%      Prioritized Planning (any-time) & 20 & 12 & 11 & 4 & 5\\
%  \cline{1-6}
%      Prioritized Planning (offline)  & 20 & 10 & 15 & 6 & 4\\
%  \hline
%  \end{tabular}
%
%\end{table*}


\emph{Discussion of Experimental Results:} Experimental results are summarized in tables \ref{table:A}-\ref{table:B}. In iNash all robots tend to bear the burden of conflict resolution similarly, on average. This contrasts with the prioritized methods, in which robot's with lower IDs have shorter paths and reach the goal more frequently than robots with higher IDs.
%
The result that some iNash paths are longer than the prioritized paths is expected, given that in iNash robots act in their own self interest. %In contrast, in the prioritized algorithms all robots follow a set of rules, and so the difference in performance between the prioritized methods and iNash can be considered the price of anarchy.


\section{Conclusions}

This paper discusses a class of multi-robot motion planning problems where each robot is associated with multiple-objectives and independent class specifications. We formulated the problem as an open-loop, non-cooperative differential game and proposed a distributed, anytime algorithm to compute the Nash equilibrium. Techniques from Rapidly-exploring Random Graphs and iterative better response were used to provide convergence guarantees and analyse the price of stability as well as the communication cost of the algorithm. We also presented results of simulation experiments that demonstrate the efficiency and anytime nature of the algorithm. Future directions include coupled task specifications of robots and algorithms which can eliminate the price of anarchy.

\bibliographystyle{plain}
%\bibliography{MZ}

\begin{thebibliography}{10}

\bibitem{Arsie.Savla.ea:TAC09}
A.~Arsie, K.~Savla, and E.~Frazzoli.
\newblock Efficient routing algorithms for multiple vehicles with no explicit
  communications.
\newblock {\em IEEE Transactions on Automatic Control}, 54(10):2302--2317,
  2009.

\bibitem{GA-JRM-JSS:07a}
G.~Arslan, J.~R. Marden, and J.~S. Shamma.
\newblock Autonomous vehicle-target assignment: {A} game theoretic formulation.
\newblock {\em ASME Journal on Dynamic Systems, Measurement, and Control},
  129(5):584--596, 2007.

\bibitem{JPA:09}
J.P. Aubin.
\newblock {\em Viability theory}.
\newblock Springer, 2009.

\bibitem{JPA-AB-PSP:11}
J.P. Aubin, A.~Bayen, and P.~Saint-Pierre.
\newblock {\em Viability theory: {N}ew directions}.
\newblock Springer-Verlag, New York, 2011.

\bibitem{baier2008principles}
Christel Baier, Joost-Pieter Katoen, et~al.
\newblock {\em Principles of model checking}, volume 26202649.
\newblock MIT press Cambridge, 2008.

\bibitem{MB-ICD:97}
M.~Bardi and I.~Capuzzo-Dolcetta.
\newblock {\em Optimal control and viscosity solutions of
  {H}amilton-{J}acobi-{B}ellman equations}.
\newblock Birkh\"{a}user, 1997.

\bibitem{MB-MF-PS:99}
M.~Bardi, M.~Falcone, and P.~Soravia.
\newblock Numerical methods for pursuit-evasion games via viscosity solutions.
\newblock {\em Annals of the International Society of Dynamic Games}, 4:105 --
  175, 1999.

\bibitem{TB-GO:99}
T.~Basar and G.~Olsder.
\newblock {\em Dynamic noncooperative game theory}.
\newblock SIAM Classics in Applied Mathematics, 1999.

\bibitem{Buckley:89}
S.~J. Buckley.
\newblock Fast motion planning for multiple moving robots.
\newblock In {\em IEEE International Conference on Robotics and Automation},
  pages 322--326, May 1989.

\bibitem{PC-MQ-PSP:99}
P.~Cardaliaguet, M.~Quincampoix, and P.~Saint-Pierre.
\newblock Set-valued numerical analysis for optimal control and differential
  games.
\newblock {\em Annals of the International Society of Dynamic Games}, 4(1):177
  -- 247, 1999.

\bibitem{castro2013incremental}
Luis I~Reyes Castro, Pratik Chaudhari, Jana Tumova, Sertac Karaman, Emilio
  Frazzoli, and Daniela Rus.
\newblock Incremental sampling-based algorithm for minimum-violation motion
  planning.
\newblock In {\em Proc. of 52nd IEEE Conference on Decision and Control}, 2013.

\bibitem{Erdmann.Lozano:87}
M.~Erdmann and T.~Lozano-Perez.
\newblock On multiple moving objects.
\newblock {\em Algorithmica}, 2(6):477--521, 1987.

\bibitem{Isaacs:99}
R.~Isaacs.
\newblock {\em Differential Games: A Mathematical Theory with Applications to
  Warfare and Pursuit, Control and Optimization}.
\newblock Dover, 1999.

\bibitem{Johari.Tsitsiklis:04}
R.~Johari and J.N. Tsitsiklis.
\newblock Efficiency loss in a network resource allocation game.
\newblock {\em Mathematics of Operations Research}, 29(3):407--435, 2004.

\bibitem{Kant.Zucker:86}
K.~Kant and S.W. Zucker.
\newblock Toward efficient trajectory planning: The path-velocity
  decomposition.
\newblock {\em The International Journal of Robotics Research}, 5(3):72 -- 89,
  1986.

\bibitem{SK-EF:11}
S.~Karaman and E.~Frazzoli.
\newblock Sampling-based algorithms for optimal motion planning.
\newblock {\em International Journal of Robotics Research}, 30(7):846 -- 894,
  2011.

\bibitem{Karaman.Frazzoli:12}
S.~Karaman and E.~Frazzoli.
\newblock Sampling-based algorithms for optimal motion planning with
  deterministic mu-calculus specifications.
\newblock In {\em 2012 American Control Conference}, pages 735--742,
  Montr\'{e}al, Canada, December 2012.

\bibitem{SML-JJK:00}
J.J. Kuffner and S.M. LaValle.
\newblock {RRT}-connect: An efficient approach to single-query path planning.
\newblock In {\em IEEE Conference on Robotics and Automation}, pages 995--1001,
  2000.

\bibitem{LaValle.Hutchinson:98}
S.M. LaValle and S.A. Hutchinson.
\newblock Optimal motion planning for multiple robots having independent goals.
\newblock {\em IEEE Transactions on Robotics and Automation}, 14(6):912--925,
  1998.

\bibitem{SML-JJK:01}
S.M. LaValle and J.J. Kuffner.
\newblock Randomized kinodynamic planning.
\newblock {\em The International Journal of Robotics Research}, 20(5):378--400,
  2001.

\bibitem{manna1995temporal}
Zohar Manna.
\newblock {\em Temporal verification of reactive systems: safety}, volume~2.
\newblock Springer, 1995.

\bibitem{Nisan.Roughgarden.ea:07}
N.~Nisan, T.~Roughgarden, E.~Tardos, and V.V. Vazirani.
\newblock {\em Algorithmic Game Theory}.
\newblock Cambridge University Press, 2007.

\bibitem{Reif:79}
J.H. Reif.
\newblock Complexity of the mover's problem and generalizations.
\newblock In {\em The 20th Annual IEEE Conference on Foundations of Computer
  Science}, pages 421--427, 1979.

\bibitem{Sanchez.Latombe:02}
G.~Sanchez and J.C. Latombe.
\newblock On delaying collision checking in prm planning -- application to
  multi-robot coordination.
\newblock {\em The International Journal of Robotics Research}, 21:5 -- 26,
  2002.

\bibitem{Sethian:96}
J.A. Sethian.
\newblock {\em Level Set Methods: Evolving Interfaces in Geometry, Fluid
  Mechanics, Computer Vision and Materials Science}.
\newblock Cambridge University Press, 1996.

\bibitem{Simeon.Leroy.Lauumond:02}
T.~Simeon, S.~Leroy, and J.-P. Lauumond.
\newblock Path coordination for multiple mobile robots: a resolution-complete
  algorithm.
\newblock {\em IEEE Transactions on Robotics and Automation}, 18(1):42 -- 49,
  2002.

\bibitem{PES:99}
P.E. Souganidis.
\newblock Two-player, zero-sum differential games and viscosity solutions.
\newblock {\em Annals of the International Society of Dynamic Games}, 4(1):69
  -- 104, 1999.

\bibitem{Xidias.Aspragathos:08}
E.K. Xidias and N.A. Aspragathos.
\newblock Motion planning for multiple non-holonomic robots: a geometric
  approach.
\newblock {\em Robotica}, 26:525--536, 2008.

\bibitem{Zhu.Martinez:12}
M.~Zhu and S.~Mart{\'\i}nez.
\newblock Distributed coverage games for energy-aware mobile sensor networks.
\newblock {\em SIAM Journal on Control and Optimization}, 51(1):1--27, 2013.

\end{thebibliography}

\end{document}

1402.2708.extracted_bib
{Arsie.Savla.ea:TAC09 A.~Arsie, K.~Savla, and E.~Frazzoli. \newblock Efficient routing algorithms for multiple vehicles with no explicit communications. \newblock {\em IEEE Transactions on Automatic Control, 54(10):2302--2317, 2009.
1402.2708.extracted_bib
{GA-JRM-JSS:07a G.~Arslan, J.~R. Marden, and J.~S. Shamma. \newblock Autonomous vehicle-target assignment: {A game theoretic formulation. \newblock {\em ASME Journal on Dynamic Systems, Measurement, and Control, 129(5):584--596, 2007.
1402.2708.extracted_bib
{JPA:09 J.P. Aubin. \newblock {\em Viability theory. \newblock Springer, 2009.
1402.2708.extracted_bib
{JPA-AB-PSP:11 J.P. Aubin, A.~Bayen, and P.~Saint-Pierre. \newblock {\em Viability theory: {New directions. \newblock Springer-Verlag, New York, 2011.
1402.2708.extracted_bib
{baier2008principles Christel Baier, Joost-Pieter Katoen, et~al. \newblock {\em Principles of model checking, volume 26202649. \newblock MIT press Cambridge, 2008.
1402.2708.extracted_bib
{MB-ICD:97 M.~Bardi and I.~Capuzzo-Dolcetta. \newblock {\em Optimal control and viscosity solutions of {Hamilton-{Jacobi-{Bellman equations. \newblock Birkh\"{auser, 1997.
1402.2708.extracted_bib
{MB-MF-PS:99 M.~Bardi, M.~Falcone, and P.~Soravia. \newblock Numerical methods for pursuit-evasion games via viscosity solutions. \newblock {\em Annals of the International Society of Dynamic Games, 4:105 -- 175, 1999.
1402.2708.extracted_bib
{TB-GO:99 T.~Basar and G.~Olsder. \newblock {\em Dynamic noncooperative game theory. \newblock SIAM Classics in Applied Mathematics, 1999.
1402.2708.extracted_bib
{Buckley:89 S.~J. Buckley. \newblock Fast motion planning for multiple moving robots. \newblock In {\em IEEE International Conference on Robotics and Automation, pages 322--326, May 1989.
1402.2708.extracted_bib
{PC-MQ-PSP:99 P.~Cardaliaguet, M.~Quincampoix, and P.~Saint-Pierre. \newblock Set-valued numerical analysis for optimal control and differential games. \newblock {\em Annals of the International Society of Dynamic Games, 4(1):177 -- 247, 1999.
1402.2708.extracted_bib
{castro2013incremental Luis I~Reyes Castro, Pratik Chaudhari, Jana Tumova, Sertac Karaman, Emilio Frazzoli, and Daniela Rus. \newblock Incremental sampling-based algorithm for minimum-violation motion planning. \newblock In {\em Proc. of 52nd IEEE Conference on Decision and Control, 2013.
1402.2708.extracted_bib
{Erdmann.Lozano:87 M.~Erdmann and T.~Lozano-Perez. \newblock On multiple moving objects. \newblock {\em Algorithmica, 2(6):477--521, 1987.
1402.2708.extracted_bib
{Isaacs:99 R.~Isaacs. \newblock {\em Differential Games: A Mathematical Theory with Applications to Warfare and Pursuit, Control and Optimization. \newblock Dover, 1999.
1402.2708.extracted_bib
{Johari.Tsitsiklis:04 R.~Johari and J.N. Tsitsiklis. \newblock Efficiency loss in a network resource allocation game. \newblock {\em Mathematics of Operations Research, 29(3):407--435, 2004.
1402.2708.extracted_bib
{Kant.Zucker:86 K.~Kant and S.W. Zucker. \newblock Toward efficient trajectory planning: The path-velocity decomposition. \newblock {\em The International Journal of Robotics Research, 5(3):72 -- 89, 1986.
1402.2708.extracted_bib
{SK-EF:11 S.~Karaman and E.~Frazzoli. \newblock Sampling-based algorithms for optimal motion planning. \newblock {\em International Journal of Robotics Research, 30(7):846 -- 894, 2011.
1402.2708.extracted_bib
{Karaman.Frazzoli:12 S.~Karaman and E.~Frazzoli. \newblock Sampling-based algorithms for optimal motion planning with deterministic mu-calculus specifications. \newblock In {\em 2012 American Control Conference, pages 735--742, Montr\'{eal, Canada, December 2012.
1402.2708.extracted_bib
{SML-JJK:00 J.J. Kuffner and S.M. LaValle. \newblock {RRT-connect: An efficient approach to single-query path planning. \newblock In {\em IEEE Conference on Robotics and Automation, pages 995--1001, 2000.
1402.2708.extracted_bib
{LaValle.Hutchinson:98 S.M. LaValle and S.A. Hutchinson. \newblock Optimal motion planning for multiple robots having independent goals. \newblock {\em IEEE Transactions on Robotics and Automation, 14(6):912--925, 1998.
1402.2708.extracted_bib
{SML-JJK:01 S.M. LaValle and J.J. Kuffner. \newblock Randomized kinodynamic planning. \newblock {\em The International Journal of Robotics Research, 20(5):378--400, 2001.
1402.2708.extracted_bib
{manna1995temporal Zohar Manna. \newblock {\em Temporal verification of reactive systems: safety, volume~2. \newblock Springer, 1995.
1402.2708.extracted_bib
{Nisan.Roughgarden.ea:07 N.~Nisan, T.~Roughgarden, E.~Tardos, and V.V. Vazirani. \newblock {\em Algorithmic Game Theory. \newblock Cambridge University Press, 2007.
1402.2708.extracted_bib
{Reif:79 J.H. Reif. \newblock Complexity of the mover's problem and generalizations. \newblock In {\em The 20th Annual IEEE Conference on Foundations of Computer Science, pages 421--427, 1979.
1402.2708.extracted_bib
{Sanchez.Latombe:02 G.~Sanchez and J.C. Latombe. \newblock On delaying collision checking in prm planning -- application to multi-robot coordination. \newblock {\em The International Journal of Robotics Research, 21:5 -- 26, 2002.
1402.2708.extracted_bib
{Sethian:96 J.A. Sethian. \newblock {\em Level Set Methods: Evolving Interfaces in Geometry, Fluid Mechanics, Computer Vision and Materials Science. \newblock Cambridge University Press, 1996.
1402.2708.extracted_bib
{Simeon.Leroy.Lauumond:02 T.~Simeon, S.~Leroy, and J.-P. Lauumond. \newblock Path coordination for multiple mobile robots: a resolution-complete algorithm. \newblock {\em IEEE Transactions on Robotics and Automation, 18(1):42 -- 49, 2002.
1402.2708.extracted_bib
{PES:99 P.E. Souganidis. \newblock Two-player, zero-sum differential games and viscosity solutions. \newblock {\em Annals of the International Society of Dynamic Games, 4(1):69 -- 104, 1999.
1402.2708.extracted_bib
{Xidias.Aspragathos:08 E.K. Xidias and N.A. Aspragathos. \newblock Motion planning for multiple non-holonomic robots: a geometric approach. \newblock {\em Robotica, 26:525--536, 2008.
1402.2708.extracted_bib
{Zhu.Martinez:12 M.~Zhu and S.~Mart{\'\inez. \newblock Distributed coverage games for energy-aware mobile sensor networks. \newblock {\em SIAM Journal on Control and Optimization, 51(1):1--27, 2013.