# Distributed Optimal Formation Control for an Uncertain Multiagent System in
the Plane
††thanks: 1C. Enwerem and J. Baras are with the Department of Electrical &
Computer Engineering and the Institute for Systems Research, University of
Maryland, College Park, MD 20742, USA. ††thanks:  2D. Romero is with the
Department of Electrical & Computer Engineering, University of Maryland,
College Park, MD and the University System of Maryland at Southern Maryland,
California, MD 20619, USA. Emails: {enwerem, baras<EMAIL_ADDRESS>††thanks: *This work received support in part from the Office of Naval
Research (Grant No. N000141712622), the Naval Air Warfare Center Aircraft
Division Seed Grant (Grant No. N004212120001), and from a joint award from
Microsoft Corporation, 1 Microsoft Way, Redmond, WA 98052, USA and the
Maryland Robotics Center, 3156 Brendan Iribe Center, College Park, MD 20742,
USA.

Clinton Enwerem1 John Baras1 Danilo Romero2

###### Abstract

In this paper, we present a distributed optimal multiagent control scheme for
quadrotor formation tracking under localization errors. Our control
architecture is based on a leader-follower approach, where a single leader
quadrotor tracks a desired trajectory while the followers maintain their
relative positions in a triangular formation. We begin by modeling the
quadrotors as particles in the YZ-plane evolving under dynamics with uncertain
state information. Next, by formulating the formation tracking task as an
optimization problem — with a constraint-augmented Lagrangian subject to
dynamic constraints — we solve for the control law that leads to an optimal
solution in the control and trajectory error cost-minimizing sense. Results
from numerical simulations show that for the planar quadrotor model considered
— with uncertainty in sensor measurements modeled as Gaussian noise — the
resulting optimal control is able to drive each agent to achieve the desired
global objective: leader trajectory tracking with formation maintenance.
Finally, we evaluate the performance of the control law using the tracking and
formation errors of the multiagent system.

###### Index Terms:

multiagent systems, unmanned aerial vehicles, swarm coordination, formation
control, optimal control.

Figure 1: Block Diagram of our proposed control architecture.

##  I Introduction

The task of formation control is central to many problems in multiagent
coordination and cooperative control, as it is usually the first problem one
typically has to solve to achieve some collective objective with multiple
agents. In the standard formation control problem, it is usually of interest
to control a group of agents — so that they converge to unique terminal states
and with the goal of attaining a desired geometric pattern — to facilitate a
specific task. Such a control objective finds direct application in several
areas such as reconnaissance, aerial coverage and monitoring, mobile target
tracking, and in mobile communication network maintenance, to name a handful.
In problems involving formation control, the prevailing assumptions are
usually that all the agents have either the same forward velocity [1, 2] or
angular velocity [3], and that information about the state of each agent is
available to its neighbors. The agents obtain this state information from
either a central station broadcasting to all agents or from a more complex
distributed network topology that can be fixed, stochastic, or even have
intrinsic dynamics [4].

Conventionally, the formation control problem takes one of two broad forms:
group reference formation control and non-group reference formation control
[4]. Group-reference formation control, also known as formation tracking, is
the case where the agents move in formation while tracking a reference
trajectory or group reference. In non-group reference formation control on the
other hand, the agents are tasked with maintaining a specific geometric shape
without following any trajectory setpoint.

Unsurprisingly, much of the research on formation control is centered around
the more challenging problem of formation tracking, and several methods have
been proposed (see [5] for a detailed survey on the topic). There is the well-
researched leader-follower paradigm where one agent is taken as the leader and
the other agents, as followers, that must track the leader’s motion while
maintaining some pre-specified distance from themselves and the leader.
Defining rules that govern the evolution of these inter-agent distances thus
leads to the desired formation, and by varying the rules, a new formation
results. Simultaneous tracking under this formation is then achieved by
specifying the desired trajectory as the leader’s path setpoint.

To effectively track the leader, follower agents require sufficiently accurate
estimates of the leader’s pose in the inertial frame, which can be affected by
noisy sensor measurements, exogenous disturbances from the environment, such
as wind or downwash from nearby agents — in the case where the agents are
aerial vehicles — or even uncertainty in the communication network from delays
and packet drops. Thus, it is often the case that the multiagent system (MAS)
will fail to track the reference trajectory while keeping formation, or
deviate from the desired formation altogether, causing unintended and even
unsafe effects [6]. Furthermore, the disturbances themselves may be difficult,
computationally expensive, or impossible to estimate, making formation
tracking under uncertainty both a safety-critical requirement and a nontrivial
problem.

Several studies have approached the formation tracking problem from an optimal
control viewpoint. One of the earliest efforts at formulating the tracking
with formation maintenance task as an optimal control problem was presented in
[7]. Here, using an approach derived from the Riccati equation, the authors
designed a distributed optimal formation control law — for multiple UAVs with
linear models — by minimizing a non-quadratic cost function. The optimal
control formulation was given here, however, without any consideration to the
pairwise distances between agents. Following standard thinking based on
Pontryagin’s Minimum Principle (PMP), the authors in [8] presented an optimal
formation control approach by minimizing the control energy of the system,
with the agents evolving under perfect-state dynamic models. More recently, an
identifier-critic-actor reinforcement learning based method was employed in
[9] to select the optimal control policy for an MAS comprising agents with
unknown and adaptively-identified nonlinear dynamics.

In our work, we study the problem of formation tracking under localization
errors where the leader in the MAS is required to track a sinusoidal
reference. Simultaneously, the followers are required to keep their assigned
planar positions with respect to the leader and themselves as defined by a
triangular formation rule. In contrast to the aforementioned research
articles, our work focuses on designing optimal formation tracking laws for a
specific case where the agents are modeled as quadrotors in the plane under
uncertainty (from sensor noise). We also formulate the formation tracking task
as a dynamic optimization problem with a constrained-augmented Lagrangian and
solve it using optimization software tools, as opposed to traditional
analytical optimal control methods like PMP or the Riccati equation.

###  I-A Contributions

Our contributions are as follows:

  1. i.

Application of optimal control theory to a uniquely-formulated multiagent
formation tracking problem.

  2. ii.

Simulative validation of the effectiveness of the optimal control law in both
the nominal setting and the case with Gaussian noise in state measurements.

In what follows, we introduce the notation used in this work and discuss the
setting under which we study the formation tracking problem (see Section II).
Next, in Section III, we provide details about the planar quadrotor model
under consideration. Section IV puts forward the optimal control component of
our work. Following that, in Section V, we discuss motivations for electing a
triangular formation as the reference formation in our work, along with a
brief description of the properties of this desired formation. The simulation
setup is provided in Section VI, with key simulation results following in
Section VII. Finally, we conclude the paper with recommendations for future
research in Section VIII.

##  II Preliminaries

We pose the formation tracking problem, as considered in this article, under
the following assumptions:

  1. i.

All agents are homogeneous, i.e., they are identical, hence (iii) follows.

  2. ii.

All but one (randomly-chosen) agent (the leader) belong to the follower group;
information about the leader’s state is available to all the follower agents
through a common communication network shared by all agents.

  3. iii.

The model of each agent can be approximated by a linear time-invariant
continuous-time model. See Section III for a description of the model.

  4. iv.

Each agent’s roll angle – and thus, rate – is approximately zero, i.e., the
agent moves to its position in the formation by maintaining a near-hover
state.

  5. v.

Each follower agent is driven independently to execute the local task of
keeping its pre-assigned position in the inertial frame and also to
simultaneously achieve the collective task of maintaining a desired group
formation with the other agents. This assumption implies that there are no
adversarial agents within the group.

  6. vi.

The uncertainty in the MAS is only due to localization errors from the state
estimation module (see Figure 1), hence the agents’ states are perturbed by
sensor noise, and are thus taken to be imperfect. Effects from external
disturbances such as wind gust and downwash are neglected.

We denote the $i^{\text{th}}$ agent as $a_{i}\in\mathcal{A}$, where
$\mathcal{A}$ is the set of all agents. $a_{\mathcal{L}}\in\mathcal{A}$
denotes the leader agent, while the follower agents are in the set
$\mathcal{A}\setminus\\{a_{\mathcal{L}}\\}$. Additionally, while it is
possible to segment $\mathcal{A}$ into a finite number of leader-follower
subsets (e.g., in the multi-leader case [10]), we have assumed that there is
only one leader (see assumption (ii)) and that all other agents are followers
within any optimization horizon. A few other assumptions will be introduced in
later sections as we specify the notation required for their definition.
However, with the above setting, we can now present the formation tracking
problem as follows: Given $N$ agents in total, $N-1$ followers must keep their
positions in the formation while the randomly-selected leader tracks a
particular trajectory in space, with possibly inaccurate state information
from sensor measurements. Essentially, we require that the group formation be
preserved, with the least possible formation error, while the leader agent
tracks a specified trajectory.

##  III Planar Quadrotor Model

We model the agents as quadrotors in the plane (see Figure 2), governed by the
dynamics of a planar quadrotor linearized at the equilibrium (hover) state:

$\displaystyle\ddot{y}_{i}=-g\phi_{i}$ (1a)
$\displaystyle\ddot{z}_{i}=-g+\frac{u_{1_{i}}}{m}$ (1b)
$\displaystyle\ddot{\phi}_{i}=\frac{u_{2_{i}}}{I_{xx}},$ (1c)

where $m$ is the mass of each agent, $g$ is the gravitation constant, and
$I_{xx}$ is the $x$ component of the (diagonal) inertia matrix. To simulate
sensor noise, we introduce White Gaussian Noise (WGN) terms to the formulation
in (1) to get:

$\displaystyle\ddot{y}_{i}=-g\phi_{i}+{w}_{1}$ (2a)
$\displaystyle\ddot{z}_{i}=-g+\frac{u_{1_{i}}}{m}+{w}_{2}$ (2b)
$\displaystyle\ddot{\phi}_{i}=\frac{u_{2_{i}}}{I_{xx}}+{w}_{3},$ (2c)

We can now write the planar quadrotor model in state-space form as:

$\dot{{\mathbf{x}}}_{i}=f_{i}({{\mathbf{x}}_{i}},{\mathbf{u}}_{i},\mathbf{w})={\mathbf{A}}{\mathbf{x}}_{i}+{\mathbf{B}}{\mathbf{u}}_{i}+{\mathbf{G}}_{c}g+{\mathbf{K}}{\mathbf{w}}.$
(3)

where ${\mathbf{A}}$ and ${\mathbf{B}}$ are the plant and input matrices of
the state-space model with appropriate dimensions, respectively, with
$\text{det}({\mathbf{A}})\neq 0$. ${\mathbf{G}}_{c}$ is the vector
$\begin{bmatrix}0&0&0&0&-1&0&0\end{bmatrix}^{T}$, which accounts for gravity
compensation in the $z_{\mathcal{O}}$-direction.
${\mathbf{K}}\in\mathbb{R}^{6\times 6}$ is the noise gain matrix while
${\mathbf{w}}\in\mathbb{R}^{6}$ is the vector
$\begin{bmatrix}0&w_{1}&0&w_{2}&0&w_{3}\end{bmatrix}^{T}$, where each $w_{j}\
(j=[1,2,3]$) follows a Gaussian distribution with mean $\mu\in\mathbb{R}$ and
standard deviation, $\sigma\in\mathbb{R}$. Table I, partly adapted from [11],
lists the discussed parameters for the simulated planar quadrotor.

TABLE I: Planar Quadrotor Model Parameters Parameter | Value
---|---
$m$ [kg] | 0.028
$I_{xx}$ [kg$m^{2}$] | $6.4893\cdot 10^{-6}$
$[\mu\ \sigma]$ | $[0.0\ 0.2]$

##  IV Optimal Quadrotor Formation Control

To achieve the formation tracking task as set forth in Section II, we solve
the following initial-value, finite-horizon optimal control problem (FHOCP)
for the $i^{\text{th}}$ agent’s control input, ${\mathbf{u}}_{i};\
i=[1,2,\dots,N]$, on the interval $\tau=[0,T]$:

$\displaystyle\min_{{\mathbf{u}}_{i}}\quad$ $\displaystyle J_{i}$ subject to:
$\displaystyle\dot{{\mathbf{x}}}_{i}(\tau)=f_{i}({{\mathbf{x}}_{i}}(\tau),{\mathbf{u}}_{i}(\tau),{\mathbf{w}})$
(4a) $\displaystyle{{{\mathbf{x}}}_{i}}(0)={\mathbf{x}}_{i}^{0},$ (4b)
$\displaystyle\big{\lvert}\big{\lvert}{\mathbf{\Gamma}}_{i}(\tau)-{\mathbf{\Gamma}}_{j}(\tau)\big{\rvert}\big{\rvert}_{2}=d_{ij}^{r};\
i\neq j$ (4c) $\displaystyle\lvert{u}_{1_{i}}\rvert\leq
u_{1_{\text{max}}};\quad\lvert{u}_{2_{i}}\rvert\leq u_{2_{\text{max}}}.$ (4d)

Here, $J_{i}$ is the objective for the $i^{\text{th}}$ agent equal to the
total expectation of the trajectory error, control, and Mayer costs defined
as:

$\mathbb{E}\Bigg{[}\int_{\tau=0}^{T}L_{i}({{\mathbf{\Gamma}}_{i}}[\tau],{\mathbf{u}}_{i}[\tau])d\tau+h(\mathbf{x}_{i}(T))\Bigg{]},$
(5)

where
$L_{i}({{\mathbf{x}}_{i}}[\tau],{\mathbf{u}}_{i}[\tau]:\mathbb{R}^{2}\times\mathbb{R}^{2}\mapsto\mathbb{R}$
is the Lagrangian defined as follows (the $\tau$ argument has been omitted for
brevity):

${\mathbf{u}}_{i}^{T}{\mathbf{R}}_{i}{\mathbf{u}}_{i}+({\mathbf{\Gamma}}_{i}-{{\mathbf{\Gamma}}_{i}}^{r})^{T}{\mathbf{Q}}_{i}({\mathbf{\Gamma}}_{i}-{{\mathbf{\Gamma}}_{i}}^{r}),$
(6)

and $h:\mathbb{R}^{6}\mapsto\mathbb{R}$ is the terminal (Mayer) cost for the
$i^{\text{th}}$ optimal control problem, given as:

$h({{\mathbf{x}}_{i}}(T))={{\mathbf{x}}_{i}}^{T}(T){\mathbf{P}}_{i}{{\mathbf{x}}_{i}}(T).$
(7)

In (5), $\mathbb{E}$ is the expectation operator — defined in terms of the
instantaneous probabilities $p(s=s(*(\tau))$ — as:

$\mathbb{E}[s(*)]=\int_{\tau=0}^{T}{s(*(\tau))p(s)}d\tau,$ (8)

where $*$ here represents a generic time-dependent argument of $s$, a generic
function. In the preceding equations, we denote the set of admissible control
laws or policies as $\mathcal{U}\subseteq\mathbb{R}^{2}$. $T\in\mathbb{R}$ is
the time horizon for the optimal control problem, and
${{\mathbf{x}}_{i}}=\begin{bmatrix}y_{i}&\dot{y_{i}}&z_{i}&\dot{z_{i}}&\phi_{i}&\dot{\phi_{i}}\end{bmatrix}^{T}\in\mathbb{R}^{6}$
is the state of the $i^{\text{th}}$ agent. $y_{i}$ and $z_{i}$ are the
respective positions of the $i^{\text{th}}$ agent along the $Y$ and $Z$
inertial axes, while $\phi_{i}$ is its roll angle. The ($\dot{\quad}$)
variables in $\mathbf{x}_{i}$ represent the corresponding linear ($y$ and $z$)
and roll rates. ${{\mathbf{x}}_{i}}^{r}\in\mathbb{R}^{6}$ is the
$i^{\text{th}}$ agent’s reference state,
${\mathbf{u}}_{i}=\begin{bmatrix}u_{1_{i}}&u_{2_{i}}\end{bmatrix}\in\mathcal{U}$
is the $i^{\text{th}}$ control, with $u_{1_{i}}$ and $u_{2_{i}}$ respectively
equal to the effective gravity-opposing force produced by the propellers of
the $i^{\text{th}}$ agent and the torque about the suppressed inertial $X$
axis. $\mathbf{\Gamma}_{i}$ is the $i^{\text{th}}$ agent’s trajectory equal to
the vector $\begin{bmatrix}y_{i}&z_{i}\end{bmatrix}^{T}\in\mathbb{R}^{2}$
while $d_{ij}^{r}\in\mathbb{R}_{+}$ is the prescribed inter-agent distance,
which can be thought of as representing the limited communication range
between agents or as a simple inter-agent proximity constraint for collision
avoidance. $\mathbf{x}_{i}^{0}$ is the initial state of the $i^{\text{th}}$
agent. ${\mathbf{P}}_{i}\in\mathbb{R}^{6\times 6}$ is the weight matrix of the
$i^{\text{th}}$ terminal cost, ${\mathbf{R}}_{i}\in\mathbb{R}^{2\times 2}$ is
the weight matrix corresponding to the control cost, and
${\mathbf{Q}}_{i}\in\mathbb{R}^{2\times 2}$ is the weight matrix for the cost
corresponding to the $i^{\text{th}}$ trajectory error
$({\mathbf{\Gamma}}_{i}-{{\mathbf{\Gamma}}_{i}}^{r})$, with the
$i^{\text{th}}$ time-varying trajectory ${{\mathbf{\Gamma}}_{i}}^{r}(\tau)$ as
reference. ${\mathbf{P}}_{i}$ and ${\mathbf{R}}_{i}$ are taken to be positive
definite ($p.d.$) matrices, while ${\mathbf{Q}}_{i}$ is chosen as follows:

${\mathbf{Q}}_{i}\ is\ \begin{cases}p.d.,&\text{if }a_{i}=a_{\mathcal{L}}\\\
0\in\mathbb{R}^{2\times 2},&\text{otherwise. }\end{cases}$

Figure 2: Planar Quadrotor Model and Coordinate Frames: $\mathcal{O}$ is the
origin of the inertial frame. A moving body frame, subscripted by
$\mathcal{B}$ and pictured in blue, is attached to the agent’s center of mass.
$u_{1}$ and $u_{2}$ retain their former definitions.

Since the desired position $\begin{bmatrix}y_{i}&z_{i}\end{bmatrix}$ in the
$YZ$-plane encodes the trajectory of the $i^{\text{th}}$ agent, the choice of
Lagrangian in (6) – with ${\mathbf{Q}}_{i}$ as defined – ensures that the
leader tracks a specific trajectory determined by a high-level trajectory
planner (see Figure 1), while the other agents maintain their position in the
formation. We define this desired formation by specifying rules that guide the
inter-agent distances between the leader and follower agents and between the
follower agents themselves (see Section V). We also set an upper bound on the
magnitude of the control signals for each agent ($u_{1_{\text{max}}}$ and
$u_{2_{\text{max}}}$), which is standard in practice.
$f_{i}:\mathbb{R}^{6}\times\mathbb{R}^{2}\mapsto\mathbb{R}^{6}$ is the
continuous linear time-variant state-space model describing the
$i^{\text{th}}$ agent, presented in Section III.

With the model in (3), we rewrite the optimal control problem (4) in a more
compact fashion as:

$\displaystyle\begin{split}\min_{{\mathbf{u}}_{i}}\quad&J_{i}+\
\lambda_{i}\cdot(d_{ij}^{r}-\big{\lvert}\big{\lvert}{\mathbf{\Gamma}}_{i}(t)-{\mathbf{\Gamma}}_{j}(t)\big{\rvert}\big{\rvert}_{2})\end{split}$
subject to:
$\displaystyle{\dot{\mathbf{x}}_{i}}(\tau)=f_{i}({{\mathbf{x}}_{i}}(\tau),{\mathbf{u}}_{i}(\tau),{\mathbf{w}})$
(9a) $\displaystyle{{\mathbf{x}}_{i}}(0)={\mathbf{x}}_{i}^{0}$ (9b)
$\displaystyle\lvert{u}_{1_{i}}\rvert\leq
u_{1_{\text{max}}};\quad\lvert{u}_{2_{i}}\rvert\leq u_{2_{\text{max}}},$ (9c)

where we have introduced the inter-agent distance constraint as a penalty term
in $J_{i}$. The objective function in (9) is the $i^{\text{th}}$ augmented
Lagrangian. $\lambda_{i}$ is a non-negative real term that specifies whether
the inter-agent distance constraint is taken into account in the
$i^{\text{th}}$ optimal control problem, and to what degree if so. Thus, we
set the value for $\lambda_{i}$ as follows:

$\lambda_{i}=\begin{cases}0,&\text{if }a_{i}=a_{\mathcal{L}}\\\
\beta>0,&\text{otherwise.}\end{cases}$

With this problem formulation and choice of $\lambda_{i}$, only the leader
tracks the desired trajectory, while the follower agents simply keep their
respective positions in the formation as determined by the triangular
formation rule and corresponding inter-agent distances.

##  V Formation Specification

In addition to tight trajectory tracking, we require the MAS to maintain a
triangular formation. We elect this formation because it is geometrically well
suited to the leader-follower concept and also ensures that the followers are
uniformly distributed spatially on a line segment behind the leader. This
pattern has been shown to be locally asymptotically stable under the
assumption that the formation is infinitesimally rigid [12].

To this end, we require that the formation be rigid, and translation and
rotation invariant, i.e., that:

$\big{\lvert}\big{\lvert}{\mathbf{\Gamma}}_{i}-{\mathbf{\Gamma}}_{j}\big{\rvert}\big{\rvert}_{2}=d_{ij}^{r}\
\forall\ i,j\in\\{1,2,\dots,N\\};\ i\neq j,$ (10)

and that there exists a $\bm{\xi}_{i}\in\mathbb{R}^{2}$ for the
$i^{\text{th}}$ agent such that:

$\begin{bmatrix}\mathbf{R}_{\theta}&\bm{\tau}_{d}\\\
0&1\end{bmatrix}\cdot\begin{bmatrix}\bm{\xi}_{i}\\\
1\end{bmatrix}=\begin{bmatrix}{\mathbf{\Gamma}}_{i}\\\ 1\end{bmatrix},$ (11)

respectively, for some homogeneous transformation in $SE(3)$ comprising a
fixed rotation about the $X$ axis by $\theta\in\mathbb{R}$ – encoded by the
rotation matrix $\mathbf{R}_{\theta}\in SO(2)$ – and a fixed translation by
$\bm{\tau}_{d}\in\mathbb{R}^{2}$. Figure 3 depicts three agents in triangular
leader-follower formation with the desired inter-agent distances labeled.

Figure 3: Three agents in the plane assuming a triangular leader-follower
formation. $d^{r}_{\mathcal{L}f}$ and $d^{r}_{ff}$ are the desired leader-
follower and follower-follower distances, respectively.

##  VI Simulation Studies

For simulation, we solve (4) using the Ipopt Python optimization package [13],
with the problem setup parameters outlined in Table II. $\mathbbm{1}_{n}$ is
the $n\times n$ identity matrix. In our problem setup, we also assume that the
dynamics of each agent is propagated forward in time.

TABLE II: FHOCP Setup Parameters

Parameter | $u_{1_{\text{max}}}$ | $u_{2_{\text{max}}}$ |  ${\mathbf{P}}_{i}$, ${\mathbf{K}}_{i}$ |  ${\mathbf{Q}}_{i}$, ${\mathbf{R}}_{i}$ | $d^{r}_{\mathcal{L}f}$ | $d^{r}_{ff}$ | $\beta$ | $T$
---|---|---|---|---|---|---|---|---
[N] | [N-m] |  |  |  |  | [m] | [m] |  |
Value | $1.2mg$ | $\frac{I_{xx}\pi}{10}$ | $\mathbbm{1}_{6}$ | $\mathbbm{1}_{2}$ | 0.5 | 0.5 | 1 | 10

##  VII Results & Analysis

###  VII-A Trajectory Tracking

Figures 4 and 5 show the optimal state variables and control inputs,
respectively, for the leader agent. As expected, the optimal $\phi$ and
$u_{2}$ values over the time horizon are approximately zero implying near
hover state. Concerning tracking performance, we can see from Figure 7(a) that
the optimal trajectory closely tracks the desired sinusoidal reference
trajectory. As expected, in the case with no measurement noise, a much better
tracking performance is recorded – near zero trajectory error and hence, tight
trajectory tracking (Figure 6). For a numerical comparison, Table III presents
root-mean-square error (RMSE) values for the leader agent’s trajectory error,
along with those for the error between the desired and actual inter-agent
distances for both follower agents (abbreviated as $f_{1}$ and $f_{2}$).

Figure 4: Time evolution of the optimal configuration for the leader agent
(model with $\mathbf{w}$). Figure 5: Optimal control inputs for the leader
agent. Figure 6: Leader trajectory error.

(a)

(b)

Figure 7: (a) Optimal leader agent trajectory with a sinusoidal reference (for
both model cases) and (b) A snapshot demonstrating the planar formation
tracking performance for the model with $\mathbf{w}$ (over $T$).

###  VII-B Triangular Formation Maintenance

Results that portray the inter-agent distance maintenance results are
presented in Figure 7(b) and Table III, from where we observe that, over the
time horizon, the triangular formation is maintained as the leader closely
tracks the desired trajectory. Upon the introduction of the WGN term, a
comparable performance is recorded, with only slight deviations from the
trajectory reference and inter-agent distance constraints — evidenced also by
the low RMSE values obtained for all three agents. A sample simulation video
(showing the formation tracking performance) is available at the following
link: https://youtu.be/aJJXmN3UJoQ.

TABLE III: RMSE values for the leader and follower agents Agent | RMSE
---|---
Nominal Model | Model with ${\mathbf{w}}$
Leader | 1.18$\times 10^{-7}$ | 0.0836
$f_{1}$ | 1.34$\times 10^{-12}$ | 1.4$\times 10^{-9}$
$f_{2}$ | 2.6$\times 10^{-9}$ | 1.3$\times 10^{-9}$

##  VIII Conclusion & Future Work

This paper presented results from formation tracking experiments for a special
class of multiagent systems: quadrotors in the plane, restricted to states
near the equilibrium. We showed that for the studied model, an overall
acceptable tracking performance was recorded for the MAS, even with simulated
white Gaussian noise in the model. In future work, we will apply similar ideas
to the case where the agents’ states evolve in space under the standard
nonlinear quadrotor model. Another promising direction would be to study how
the performance of the optimal control scheme is affected by scaling the
number of agents.

## Acknowledgements

The authors gratefully acknowledge Microsoft Corporation and the Maryland
Robotics Center for their kind financial support.

## References

  * [1] L. Ma and N. Hovakimyan, “Cooperative Target Tracking with Time-Varying Formation Radius,” in 2015 European Control Conference (ECC), pp. 1699–1704, July 2015.
  * [2] J. A. Marshall, M. E. Broucke, and B. A. Francis, “Formations of vehicles in cyclic pursuit,” IEEE Transactions on Automatic Control, vol. 49, pp. 1963–1974, 2004.
  * [3] L. Briñón Arranz, A. Seuret, and C. Canudas de Wit, “Contraction control of a fleet circular formation of AUVs under limited communication range,” in Proceedings of the 2010 American Control Conference, pp. 5991–5996, June 2010.
  * [4] Y. Cao, W. Yu, W. Ren, and G. Chen, “An Overview of Recent Progress in the Study of Distributed Multi-Agent Coordination,” IEEE Transactions on Industrial Informatics, vol. 9, pp. 427–438, Feb. 2013.
  * [5] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, Mar. 2015.
  * [6] T. Liu, M. de Queiroz, P. Zhang, and M. Khaledyan, “Further results on the distance and area control of planar formations,” International Journal of Control, vol. 94, pp. 767–783, Mar. 2021.  Publisher: Taylor & Francis _eprint: https://doi.org/10.1080/00207179.2019.1616824.
  * [7] J. Wang and M. Xin, “Integrated Optimal Formation Control of Multiple Unmanned Aerial Vehicles,” IEEE Transactions on Control Systems Technology, vol. 21, pp. 1731–1744, Sept. 2013.
  * [8] Y. Liu and Z. Geng, “Finite-time formation control for linear multi-agent systems: A motion planning approach,” Systems & Control Letters, vol. 85, pp. 54–60, Nov. 2015.
  * [9] G. Wen, C. L. P. Chen, and B. Li, “Optimized Formation Control Using Simplified Reinforcement Learning for a Class of Multiagent Systems With Unknown Dynamics,” IEEE Transactions on Industrial Electronics, vol. 67, pp. 7879–7888, Sept. 2020.
  * [10] N. Sorensen and W. Ren, “A Unified Formation Control Scheme with a Single or Multiple Leaders,” in 2007 American Control Conference, pp. 5412–5418, July 2007.  ISSN: 2378-5861.
  * [11] J. Förster, “System Identification of the Crazyflie 2.0 Nano Quadrocopter,” bachelor’s thesis, ETH Zurich, Aug. 2015.
  * [12] K.-K. Oh and H.-S. Ahn, “Formation control of mobile agents based on inter-agent distance dynamics,” Automatica, vol. 47, pp. 2306–2312, Oct. 2011.
  * [13] A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, pp. 25–57, Mar. 2006.


scaletikzpicturetowidth[1]

# Pedestrian-Robot Interactions on Autonomous Crowd Navigation:
Reactive Control Methods and Evaluation Metrics

Diego Paez-Granados1†, Yujie He2, David Gonon2, Dan Jia3, Bastian Leibe3,
Kenji Suzuki4, Aude Billard2 *This work was funded in part by the EU H2020
project ”Crowdbot” (779942).† is the corresponding author. 1 D.Paez-Granados
is with the SCAI Lab, Swiss Federal School of Technology in Zurich - ETH
Zurich and SPZ, Switzerland<EMAIL_ADDRESS>Y. He, D. Gonon, and A. Billard are
with the LASA Laboratory, Swiss Federal School of Technology in Lausanne -
EPFL, Switzerland {yujie.he; david.gonon<EMAIL_ADDRESS>D. Jia, and
B. Leibe are with the Visual Computing Institute, RWTH, Germany {jia;
<EMAIL_ADDRESS>K. Suzuki is with the Artificial Intelligence
Lab, University of Tsukuba, Japan<EMAIL_ADDRESS>
###### Abstract

Autonomous navigation in highly populated areas remains a challenging task for
robots because of the difficulty in guaranteeing safe interactions with
pedestrians in unstructured situations. In this work, we present a crowd
navigation control framework that delivers continuous obstacle avoidance and
post-contact control evaluated on an autonomous personal mobility vehicle. We
propose evaluation metrics for accounting efficiency, controller response and
crowd interactions in natural crowds. We report the results of over 110 trials
in different crowd types: sparse, flows, and mixed traffic, with low- ($<0.15\
ppsm$), mid- ($<0.65\ ppsm$), and high- ($<1\ ppsm$) pedestrian densities. We
present comparative results between two low-level obstacle avoidance methods
and a baseline of shared control. Results show a 10% drop in relative time to
goal on the highest density tests, and no other efficiency metric decrease.
Moreover, autonomous navigation showed to be comparable to shared-control
navigation with a lower relative jerk and significantly higher fluency in
commands indicating high compatibility with the crowd. We conclude that the
reactive controller fulfils a necessary task of fast and continuous adaptation
to crowd navigation, and it should be coupled with high-level planners for
environmental and situational awareness.

###### Index Terms:

Mobile Service Robots, Human-Robot Interaction, Reactive Navigation Control,
People detection and tracking, autonomous navigation

##  I Introduction

Mobile service robots offer great societal value, such as transporting
personal mobility devices (Segway, USA, Rokuro, Japan), last-mile delivery
services (Starship Inc. USA), autonomous cleaning robots (Bluebotics,
Switzerland), autonomous wheelchairs (Whill Inc. Japan), telepresence robots
and tour-guide robots. Nonetheless, most robots are still limited to
navigation in low-density areas, such as pathways or large open areas with the
basic safety control system setting the robot to freeze as soon as it
perceives a likely contact with pedestrians [1, 2]. Such a reaction would most
likely be unexpected by pedestrians and lead to more dangerous collisions with
pedestrians stumbling on the robot, a ”frozen” robot would become a danger to
itself and bystanders [3, 4]. This is the case, in highly dynamic environments
such as malls, airports, markets, and mix-traffic zones with other mobility
vehicles (as shown in Fig. 1).

However, guaranteeing obstacle avoidance during navigation in highly occupied
areas would be unattainable for current mobile service robots bounded by
actuation power, computational resources, and expected to behave as
pedestrians, i.e., holonomic, reactive, communicative, and knowledgeable of
proxemics and other social rules. In this work, we investigate possible
reactive control strategies that avoid the ”freezing” robot problem and
experimentally validate the compatibility and interaction with bystander
pedestrians.

Figure 1: Crowd navigation evaluation around the city of Lausanne, with the
robot Qolo using reactive navigation for obstacle avoidance and post-contact
compliance. Top, depicts the on-board people detection and tracking from Lidar
and RGBD data. Bottom, navigation view from the external camera in dense mixed
crowds environment

Different control approaches have been proposed for dynamic obstacle
avoidance, from model predictive control (MPC) applied as a quadratic program
for avoiding collisions (combining braking and steering) [5], or computing
interactions with other agents and formulating crowd navigation as an optimal
control problem [6]. As well, the partially observable Markov decision process
(POMDP) is a common framework for planning with other agents’ uncertain
intentions [7, 8, 9]. Other works in path planning solve the dynamic
environments through models of the crowd trained in simulation. The work in
[2] uses deep reinforcement learning to plan the robot’s trajectory through a
surrounding crowd’s motion which is tracked and predicted by a separate
algorithm beforehand. [10] works similarly with predictions of zones where the
robot could become obstructing or freeze, and the approach then avoids these
zones. While the work in [11] presents a time efficiency-based method for path
planning in crowd environments. Nonetheless, none of these works guarantees
continuous operation of the robot navigation or offers a solution for fully
blocked passages, likely to occur in dense crowds. Moreover, none of these
methods has been systematically evaluated in natural crowds.

Dynamical systems (DS) based obstacle avoidance provides a fast and continuous
solution through modulation of obstacles, as offered in [12]. We have
augmented this approach with a compliant control mechanism, herewith
mitigating risks and allowing the robot to navigate in post-contact scenarios
through sliding behaviour, as proposed in [13]. This approach offers the
reactivity of time-invariant DS combined with contact estimations of impacts
through a compliant bumper, herewith ensuring impact absorption through
passive and active compliance for mitigating unexpected impacts with mobile
robots.

In this work, we provide a framework for crowd navigation through an
integrated reactive controller for obstacle avoidance and post-contact sliding
control. We contribute by providing a systematic evaluation methodology and
metrics for robot assessment on crowd navigation tasks. We present the results
from testings in multiple natural crowd densities: low ($<0.15\ ppsm$), medium
($<0.65\ ppsm$), and high ($<1\ ppsm$), and crowd types: sparse, flows and
mixed traffics.

Previous works on crowd navigation have focused on three main metrics:
collisions, success rate, and time to goal [14, 11]. While the work in [11],
proposed a mid-density flow evaluation ($<0.5ppsm$) with volunteer
participants in a controlled setting, and used time efficiency and boundary
violations (virtual collisions) as main metrics. In contrast, we propose
evaluations on natural crowds with additional metrics that assess path
efficiency, controller performance, and robot-crowd interaction, similar to
the simulation framework offered in [15].

We validated the whole architecture for crowd navigation and three reactive
navigation controllers on a personal mobility vehicle - Qolo [16] shown in
Fig. 1, a type of powered wheelchair for standing mobility of lower-limb
impaired people. Although results in this work were shown on a person carrier
robot, the proposed controller and metrics are equally valid for any mobile
robot. We provide the whole dataset of the current experiments as open-access
111Dataset website: https://www.epfl.ch/labs/lasa/crowdbot-dataset/ in [17].
As well as, all source code for processing and analyzing interactions 222Data
analysis tools can be found here: https://github.com/epfl-lasa/crowdbot-
evaluation-tools. Which should enable future research on understanding
people’s navigation around robots, and improving detection and tracking
methods.

##  II Problem Statement

Obstacle avoidance methods usually consider a bi-state problem with collisions
as an absolute negative state which in turn leads to the “freezing robot”
problem [1, 2]. Nonetheless, contact might be inescapable when the robot’s
kinematic and dynamic constraints are below the pedestrians. Hence, collisions
might become unavoidable even in simple scenarios. In our previous work [13],
we focused on combining active compliance with DS-based obstacle avoidance
which provides a way to slide around obstacles while in contact and continue
moving towards the goal. Sliding in contact limits force to a determined safe
threshold following safe design considerations for robot impacts [18].

In this work, we target to prove the feasibility of crowd navigation in
natural crowds through reactive navigation control and developed appropriate
metrics for its assessment. We compare two methods for obstacle avoidance and
compared their performance with shared control (SC) as a baseline with user-
given commands [19]. We explored three different controllers focusing on the
following questions:

  1. 1.

What difference can be observed in the representation and control of an
autonomous robot around actual crowds?

  2. 2.

How do the proposed reactive navigation methods perform when the crowd density
changes?

###  II-A Control Architecture for Reactive Control in Dense Crowd Navigation

In this work, we formulated the crowd navigation control by dividing it into
three main layers (as depicted in Fig. 2): First, the high-level planner
decides the motion direction and velocity. In the robot Qolo a user intention
recognition for shared control is possible through a hands-free user interface
[19], or a standard virtual joystick. Second, a reactive control layer that
deals with local obstacle avoidance and post-collision control through closed-
loop dynamics of the robot in its control space ($\dot{\xi}_{u}$). The main
objective of the reactive control module is to provide a layer for local
navigation assistance and immediate responsiveness to unmodeled or unpredicted
object occurrences or motions. Therefore, this algorithm assumes a continuous
dynamical system (DS) guiding the robot’s motion to be existing and given by a
high-level algorithm that optimally plans towards its intended goal (a user-
given command, or a DS from planners).

For obstacle avoidance, we use a Modulated Dynamical System (MDS) [12],
representing obstacles analytically as star-shaped level sets of a distance
function that absorb the robot’s footprint, thus, allowing the robot to be
represented as a point moving in Cartesian space. In the case of Qolo, we
control a single point in the bumper area as a holonomic point. It guarantees
to lead the robot to its goal (hereafter called attractor) by assuming a
circular virtual boundary (as depicted in Fig. 3). Further details of this
formulation are described in [13]. Similarly, local obstacle avoidance could
be achieved through the Reactive Driving Support (RDS) proposed in [20], which
replicates the behaviour of the MDS locally through Velocity Obstacles (VO)
based controller. While it sacrifices the guarantee to reach the global goal
(by itself, i.e., without an additional path planner), it allows representing
more accurately the robot shape detail and the non-holonomic constraints of
the robot’s kinematics.

Figure 2: Controller architecture proposed for reactive navigation control on
crowds navigation. High-level planning was set from the user in shared-control
low-level blending or full autonomy it was replaced by a linear DS towards an
attractor at the desired location (20 to 50m ahead). Obstacle avoidance used a
VO-based optimization [20] or a modulated Dynamical System (DS) [12]. Finally,
a passive DS handles the post-collision through sliding [13].

For collision control, we have proposed a compliance and contact force
controller through a sliding method using a known sensing surface over the
robot’s hull with a limited contact reference force $F_{n}$. Ensuring that the
robot reacts to unexpected contacts (in this case, limited to the frontal
bumper) and advances with a sliding manoeuvre should the underlying obstacle
avoidance lead away from the contact surface without colliding with other
obstacles, as we proposed in [13].

We modelled the robot dynamics as:$M\ddot{\xi}+C\dot{\xi}=\tau_{c}+\tau_{e}$,
where $\dot{\xi}\in\mathbb{R}^{2}$ represents the robot’s Cartesian velocity
as a time invariant position dependent dynamical system.
$M\in\mathbb{R}^{2\times 2}$ corresponds to the virtual mass of the robot,
$C\in\mathbb{R}^{2\times 2}$ accounts for centrifugal and Coriolis terms,
$\tau_{c}$ represents the control forces and $\tau_{e}$ any external
disturbances to be rejected. We use a controller of the form:

$\displaystyle\tau_{c}$ $\displaystyle=$
$\displaystyle\lambda_{t}f_{u}(\dot{\xi})+(F_{n}+F_{c})\hat{n}-D\dot{\xi}\ ,$
(1)

where $f_{u}(\xi)$ represents the driving force generated by the nominal DS
from the obstacle avoidance input, applied tangentially to the collision
surface ($\lambda_{t}$) in case of contact. $F_{n}$ represents a chosen force
limit bounded by safety and acceptability, while $F_{c}$ represents the
estimated contact force in the $\hat{n}$ normal surface.
$D\in\mathbb{R}^{2\times 2}$ represents a negative defined damping effect used
for controlled sliding during contacts. We transform the velocity to the
domain of the robot by a first-order Taylor expansion, thus the control for
the desired velocity $\dot{\xi}_{d}$ is,

$\displaystyle\dot{\xi}_{d+1}$ $\displaystyle=$
$\displaystyle\frac{T_{s}}{M}\left((F_{n}+F_{c})\hat{n}-D\dot{\xi}_{d}\right)+\hat{t}^{T}\dot{\xi}_{u}\hat{t}\
,$ (2)

where a discretizing time constant ($T_{s}$) is set to the sampling time of
the control loop, and $M$ is a virtual desired mass for impact response.

The third and final layer is a low-level control that guarantees closed-loop
velocity control and state feedback to the higher-level controllers.

A key component for crowd navigation is the localization and tracking of the
environment and crowd ($\Xi$). We have set people tracking through a real-time
pipeline of sensing fusion of Lidar-based detection by DR-SPAAM [21] and RGBD
detection by YOLO [22]. Whereas robot state ($\xi$) was estimated on optical
flow principle from a stereo camera (Intel T265) fused with IMU and odometry.
The full controller repository can be found in [23].

##  III Evaluation Method for Crowd Navigation

###  III-A Evaluation Metrics

Evaluation of performance in crowd navigation for mobile robots should account
for the robot’s performance in achieving its task, as well as the
compatibility with pedestrian navigation response and safety-related metrics.

We integrate and present feasible metrics for crowd-robot interaction from
embedded sensing and perception pipelines. Moreover, we make use of 3D point-
cloud sensing for further assessment of the robot’s navigation performance in
the crowd through 3D people detection [24], and tracking [25]. The following
metrics were considered:

####  III-A1 Path Efficiency

Relative time to goal: The relative time to goal compares the time taken by
the robot to reach its goal when it is alone to the time it takes when driving
in the crowd as: $T_{rtg}=t_{\text{free }}/t_{c}$, where $t_{\text{free }}$ is
the free path completion time, and $t_{c}$ is the robot crowd navigation time.
Thus a $T_{rtg}=1$ would be a free path or 100% efficient navigation.

Relative Path length: The relative path length compares the length of the path
taken by the robot to reach its goal with and without a crowd as:
$L_{r}=L_{goal}/L_{c}$, where $L_{goal}$ is the shortest path to the goal, and
$L_{c}$ is the traveled path with the crowd.

Relative Jerk: The relative jerk evaluates the smoothness of the path taken by
the robot to reach its goal ($J_{\text{c }}$) normalized to a reference jerk
($J_{\text{ref }}$) taken during a manual operation of the robot around the
crowd as follows:

$\displaystyle\begin{aligned} &J_{rp}=J_{\text{c }}/J_{\text{ref }}\ ,\\\
&J=\left(1/t_{f}\right)\sum_{t=0}^{t_{f}}\Delta
t\sqrt{J_{v}{}^{2}+J_{\theta}{}^{2}}\ ,\end{aligned}$ (3)

where the computed jerk $J$ takes $J_{v}$ and $J_{\theta}$ from the linear and
angular jerk, respectively. $\Delta t$ corresponds to the sample time window
over the period $0\sim t_{f}$.

####  III-A2 Controller performance

These tests aimed to estimate the level and type of assistance provided by the
reactive controller towards avoiding obstacles. We adapted these metrics from
shared control (SC) studies [26, 27, 28].

Contribution: C is calculated as $C=||u_{r}-u_{u}||/||u_{u}||$, over a finite
number of discrete samples N. Where the $u_{r}$ is the user command in the
robot command space (linear and angular speed), $U_{r}$ is the output command
of the robot. Both normalized to the maximum linear and angular velocities. In
case of full autonomy with a high-level input controller, we evaluated the
contribution as $C=((||u_{r}||)/(||u_{u}||))$ . Where $||x||$ represents the
L2-norm of $x$.

Fluency: as a measurement of the commands temporal continuity given that the
reactive navigation is intervening with the main driving DS desired motion as:

$\displaystyle
F=\frac{1}{N}\sum_{t=t_{0}}^{t_{N}}1-\left|u_{h}^{t}-u_{h}^{t-1}\right|,$ (4)

where $u_{h}$ is the high-level input from the user.

Agreement: We defined in terms of the deviation of the direction of the high-
level commands from the direction of the final control’s velocity, as follows:

$\displaystyle\begin{aligned} &\theta(u)=\tan^{-1}\left(\frac{v}{w}\right),\\\
&a_{i}=1-\left|\theta\left(z_{u}^{i}\right)\ominus\theta\left(u_{SC}^{i}\right)\right|/\pi,\\\
&\text{agreement}=\sum_{i=0}^{N}a_{i}\cdot\Delta t_{i}/\sum_{i=0}^{N}\Delta
t_{i},\end{aligned}$ (5)

where $v$ and $w$ are the linear and angular velocities $u=[v,w]$, $a_{i}$ is
the normalised agreement at time step $t_{i}$ and $u_{SC}^{i}$ is the final
output of the robot. $N$ is the number of samples available in which data from
the high-level command input $z_{u}^{i}$ coincide in time with $u_{SC}^{i}$ ,
and $\Delta t_{i}$ is the duration of the high-level input command
$z_{u}^{i}$.

####  III-A3 Pedestrian interaction metrics:

Crowd density: We report the crowd density in terms of mean, standard
deviation and max density. However, the limited field of view of the robot as
it is embedded in the crowd hinders overall measurements. Therefore, we
reported on the density within 2.5, 5 and 10m around the robot (as shown in
Fig. 5(c)), which explains better the scenario.

Minimal distance: the minimal distance to the robot held by any pedestrian.

Virtual Collisions: number of violations to the virtual boundary set to the
robot controller, as shown in Fig. 3.

Collisions: number of collisions in a scenario reported by the experimenters
and post-analysis of the recordings.

###  III-B Experimental Setup

Figure 3: Robot sensing implementation on Qolo [16]: obstacle perception with
2 LIDARs (Velodyne VLP-16), RGBD sensors (Intel Realsense D435), force sensing
with one Rokubi 2.0 (Botasys). With the perception and control implemented on
two embedded computers Upboard Squared, and one Nvidia Jetson Xavier AGX. On
the right, the boundary representations used around the actual robot

In this work, we present an evaluation of natural crowd navigation through
reactive control. We have used the robot Qolo [16] \- a person carrier mobile
robot for lower-limb impaired users - instrumented as shown in Fig. 3.

We present a comparative evaluation of two modes of obstacle avoidance (MDS
and RDS) driven by a linear DS towards a set attractor (or goal some distance
from the starting point). And compare it to a baseline of shared-control where
a user guides the high-level interaction with the crowd (effectively, deciding
the direction of motion). Such evaluation allows observing the compatibility
of the methods with the crowd, the high-level commands, and the effectiveness
of pure reactive navigation. Two navigation scenarios were developed for
different crowd densities:

####  III-B1 Scenario 1

In this scenario, we performed crowd navigation on a mixed-traffic street (Rue
de Saint-Laurent, Lausanne). The path was chosen at an intersection of 6
streets (as shown in Fig. 4, top). We encountered flows from low to mid-
density with mixed crowd types: 2D flows and sparse crowds with static
pedestrians. The sparse crowds’ mean density was between $0.05\ ppsm$ to
$0.15\ ppsm$ (40 records 50 m round trips, with 33 successful trajectories)
and a max crowd density of $0.7\ ppsm$. The experiment was repeated over
multiple dates during a farmer’s market to elicit similar crowds.

####  III-B2 Scenario 2

In this scenario, we performed 1D flow navigation during the Christmas market
(Place de l’Europe, Lausanne), as depicted in Fig. 4, bottom. Here, we found
denser mixed crowds formed by people lining up and flows of pedestrians. In
this case, we followed the same protocol of traversing a set path whose
starting and ending goals were 30 m apart.

All data were recorded on ROS standard data type (rosbag) with the following
information:

  * •

Pedestrian motion information in the form of a set of two sets of 3D point
clouds around the robot, including all surrounding people and obstacles in a
range of up to 50 m.

  * •

Pedestrian’s motion data from a forward-looking RGBD camera, with people,
labelled and blurred.

  * •

Output from 3 people detection layers and 1 integrated people tracker.

  * •

Force/Torque information gathered by the contact sensors at the robot’s
bumper.

  * •

Recordings of the navigation interface input given by the user/driver of the
robot.

  * •

Motion data was gathered from the robot inertia sensors and odometry sensors.

  * •

Video recording of the scene from the robot’s perspective without personal
identification recording.

  * •

*Video recording of the participant-robot driving in a scenario, with no personal identification.

All data captured have been released as an open-access dataset [17], with
rosbag files and post-processing data for each of the following scenarios.

Figure 4: Experimental setup on two mixed traffic streets with a fixed
starting point and goal (attractor). Scenario 1, constitutes a mixed influx of
6 streets during a farmer’s market with up to $0.7\ ppsm$. Scenario 2, is a
closed pedestrian street with crowd crowds up to $1\ ppsm$, and the
corresponding experiment was conducted during the Christmas market.

##  IV Results

(a) Snapshot of the experiment where a user onboard the robot Qolo acts as
safety operator while the robot drives in full autonomous mode to navigate 20
m ahead in the mixed crowd flow with parts of the trajectory in low-density
$<0.2ppsm$ ($t=2,12,38$), and mid-density $<0.7ppsm$ ($t=24$).

(b) Robot trajectory over one round trip to the goal location (further point
to the right). Detected pedestrians are over-imposed with labelled numbers and
corresponding colours at t=5, t=75, and t=118.6.

(c) Crowd density estimated from online sensing of people detection [21] at
2.5 m, 5.0 m, and 10 m around the robot. The time labels denote the start and
end of the first trajectory to reach the goal. The subsequent time is the
return to the starting point.

Figure 5: Experimental setup example of one of the 110 trials of point-to-
point navigation running around crowds in the city of Lausanne (Switzerland)).

Scenario 1 was successfully recorded 95 times for about 5.0 km in sets of 20 m
round trips (with 3 interrupted trials). A sample data is given in Fig.5
around low and mid crowd densities. Scenario 2 was successfully recorded 15
times with the highest densities close to $1\ ppsm$ from on-board
measurements.

###  IV-A Controller Response in Crowds

The overall crowd density (as shown in Fig. 5(c)) measured from the onboard
sensing fluctuates around $0.12\ ppsm$, with peaks of up to $0.6\ ppsm$.
Although it is worth noting that the current measurements are limited to the
proximity sensors, therefore, it could be possible that higher densities were
not visible from the robot’s viewpoint. Out of successfully recorded 16 MDS
trials, 28 RDS trials, and 48 SC trials. We qualified all data of successfully
reaching the goal with a margin of 3 m around the goal (as depicted in Fig.
4). Moreover, we took compatible trials with no significant difference in
crowd density variance among tests (mean and max). Resulting on a set of 16
MDS trials, 20 RDS trials, and 16 SC trials for comparison.

(a) Controller contribution.

(b) Controller agreement.

(c) Controller fluency.

(d) Relative Jerk.

Figure 6: Controllers evaluation among the task on scenario 1 on crowds
between low-mid densities (up to 0.6 ppsm). All tests were compared by a one-
way ANOVA at the levels $*\rightarrow p<0.1$, $**\rightarrow p<0.05$, and
$***\rightarrow p<0.01$.

In the interaction with the crowd, the minimal distance to pedestrians (Table
I) during the trials showed to be lower in the case of the RDS controller,
compared with MDS ($p<0.05$, $F=3.9$), which is expected for the RDS
controller having a more tight definition of the robot’s shape. Nonetheless,
in shared control where the obstacle avoidance is provided by RDS, there was a
significantly higher minimal distance ($p<0.05$, $F=4.4$) to surrounding
pedestrians. This suggests that the high-level input from a user would prefer
a higher proxemic distance to the pedestrians than the reactive controllers
provided ($\sim 0.5m$).

The path efficiency results show a relative time to the goal similar among
controllers, with relative times from 30% up to 60% the efficiency of a free
path. No significant difference was observed among the methods. This suggests
that the reactive navigation for obstacle avoidance does not hinder
performance when compared with SC where the user can take lead over the
direction of motion. The relative path length was similar among all recordings
of MDS ($21.5\pm 5.9m$), RDS ($20.2\pm 6.2$), and SC ($27.1\pm 12.8$ ),
showing no significant difference.

The relative jerk (Fig. 6(d)) showed significant difference between MDS and
RDS controllers ($p<0.1,F=7.3$), with RDS being 5% lower. RDS and SC mode
($p<0.01,F=41.8$), with RDS being 6% lower. On the contrary, comparing SC and
MDS did not show a significant difference.

In turn, the fluency of the controllers (Fig. 6(c)) was significantly
different among all tested controllers ($p<0.01$, $F=28.4$), however, only in
SC tests was observed a noticeable difference with a 2% drop in the fluency of
the user-robot commands. Overall, this high fluency demonstrates the
compatibility of the reactive controllers with the high-level planner.

Analyzing the controller contribution and agreement we observed that RDS
control guides most of the linear velocity components with an average 87%
contribution, whereas the MDS controller provided 60% of contribution
(significantly different at the level $p<0.01$, $F=80.6$) and agreement on the
70%. The difference in the approach to avoiding obstacles is clear, with RDS
controlling the magnitude of the velocity, whereas MDS executes higher control
over the angular component.

Compared with SC, the results (see, Fig. 6) show that the obstacle avoidance
contributed to 50%, with agreement over 85%, which means that the user was in
control of the motion direction 50% of the time, and the obstacle avoidance
assisted mostly in the velocity magnitude over the tests.

TABLE I: Controller comparison in mid-density crowds Metrics | Controller
---|---
MDS | RDS | Shared control
Avg. crowd density | $0.12\pm 0.03$ | $0.13\pm 0.03$ | $0.12\pm 0.03$
Max crowd density | $0.45\pm 0.08$ | $0.47\pm 0.12$ | $0.51\pm 0.14$
Min distance | $1.19\pm 0.16$ | $1.08\pm 0.18$ | $1.20\pm 0.16$
Time to goal | $0.28\pm 0.09$ | $0.32\pm 0.10$ | $0.29\pm 0.07$
Path length | $1.41\pm 0.21$ | $1.34\pm 0.20$ | $1.52\pm 0.52$
Jerk | $0.13\pm 0.06$ | $0.09\pm 0.03$ | $0.15\pm 0.02$
Contribution | $0.58\pm 0.12$ | $0.89\pm 0.09$ | $0.49\pm 0.21$
Avg. fluency | $0.97\pm 0.02$ | $0.99\pm 0.01$ | $0.95\pm 0.02$
Avg. agreement | $0.71\pm 0.06$ | $0.74\pm 0.04$ | $0.89\pm 0.02$
Virtual collision | $3.50\pm 2.71$ | $7.05\pm 7.92$ | $4.25\pm 3.11$
Actual collision | 2/16 | 2/20 | 3/16

####  IV-A1 Collisions:

As detailed in table I, we observed, across all tests, seven collisions
leading to contact with pedestrians. All of which occurred at the frontal
bumper of the robot. In 5 of the collisions, either the operator on-board the
robot or the external supervisor of the tests pressed the emergency stop,
therefore no post-collision control was recorded on those occasions. In the
remaining 2 contacts (contacts with the RDS model) the contact was with a
pedestrian’s shopping trolley, and the contact was below impact threshold
$F_{c}=45N$, so that, there was not any perceived reaction from the compliance
controller, nor was collision perceived by the pedestrian.

We documented each collision case and found the following possible causes for
the controller behaviour. First, limited acceleration on the robot that could
not match the pedestrian’s acceleration. Probable cause of one collision with
RDS test, and one with SC. Second, non-holonomic constraints in dense crowds
limit reactivity, a probable cause for one collision with MDS. Third,
pedestrians being very close to a surrounding obstacle generated false
negatives in pedestrian detection. Probable cause for two collisions with SC
and MDS tests.

Virtual collisions, defined as entering the virtual space of the robot
controller (as depicted in Fig. 3). We found a slightly lower number of cases
on the MDS controller compared with RDS ($p<0.1$, $F=2.9$), with no
significant difference to SC. This result is expected given the larger size of
the robot representation used in MDS, which forces the robot to be further
away from pedestrians.

###  IV-B Crowd Density Influence on Controller Response

(a) Max crowd density (2.5m).

(b) Virtual collisions (entering the robot’s safe space).

(c) Minimal distance to pedestrians.

(d) Relative time to goal.

(e) Controller contribution.

(f) Controller agreement.

Figure 7: Navigation evaluation among the task on scenarios 1 and 2 comparing
reactive navigation performance by crowd density. The cluster size was 15, 20,
and 13 for low, medium and high density, respectively. All tests were compared
by a one-way ANOVA at the levels $*\rightarrow p<0.1$, $**\rightarrow p<0.05$,
and $***\rightarrow p<0.01$.

We took 48 samples from scenarios 1 and 2 of successful crowd navigation
within the set points using the shared control navigation. Subsequently, we
cluster the data in 3 groups of low, medium and high densities considering
average, variance and max densities in $2.5m$ and $5m$, resulting in the
metrics computed in table II. The metrics with a significant difference among
the density conditions are shown in Fig. 7.

The proximity of pedestrians clearly drops consistently with the crowd density
($p<0.01$, $F=53.3$) as shown in Fig. 7(c). Meanwhile, the average number of
virtual collisions increases significantly only to medium densities ($p<0.01$,
$F=15.3$). We observe, in sparse crowds a mean of 2.9 virtual collisions per
test, whereas an average of 12 collisions per test in dense crowds.

Nonetheless, it is worth noting that in high-density crowds no actual
collisions or contacts were recorded. Contrary to a medium density where we
recorded 3 contacts.

Comparing the path efficiency, we found a significant difference ($p<0.01$,
$F=8.5$) in the relative time to goal with navigation on high density dropping
$10\%$ of the relative time. In contrast, relative path length showed no
significant difference among the tested crowd environments. Hence, we observe
an expected behaviour compatible with human crowds where the reactive
controller operates by adjusting its velocity to the crowd.

Fig. 7(e) shows the comparison of the controller contribution for the
navigation. We observe no significant difference between low-mid densities,
with only high density showing a significantly higher contribution ($p<0.05$,
$F=3.9$) with a mean of 69%. In the agreement metric with the user, we observe
significantly high values (82%) in low density ($p<0.05$, $F=5.8$), which
drops to 75% in medium density, and 72% in high density. i.e., sparse low-
density crowd navigation required less reactive control assistance whereas in
the cases of mid- and high-density required increased reactive control
contribution although no decreased agreement was registered. The user and the
controller likely agree on the direction due to the more restrictive available
space in the flow of dense crowds

TABLE II: Crowd Navigation Comparison in different densities Metrics | Crowd density
---|---
Low | Medium | High
Avg.Density (2.5m) | $0.08\pm 0.02$ | $0.18\pm 0.03$ | $0.26\pm 0.04$
Max.Density (2.5m) | $0.37\pm 0.07$ | $0.64\pm 0.06$ | $0.78\pm 0.06$
Avg.Density (5m) | $0.09\pm 0.02$ | $0.17\pm 0.02$ | $0.19\pm 0.02$
Min.Distance | $1.44\pm 0.24$ | $0.96\pm 0.13$ | $0.85\pm 0.11$
Time to goal | $0.33\pm 0.09$ | $0.30\pm 0.10$ | $0.20\pm 0.10$
Path length | $1.46\pm 0.57$ | $1.74\pm 1.28$ | $2.89\pm 4.07$
Jerk | $0.14\pm 0.03$ | $0.14\pm 0.03$ | $0.15\pm 0.03$
Contribution | $0.51\pm 0.23$ | $0.59\pm 0.14$ | $0.69\pm 0.15$
Avg. fluency | $0.95\pm 0.02$ | $0.96\pm 0.02$ | $0.95\pm 0.01$
Avg. agreement | $0.82\pm 0.11$ | $0.75\pm 0.08$ | $0.72\pm 0.05$
Virtual collision | $2.93\pm 2.94$ | $13.15\pm 19.28$ | $12.08\pm 8.50$

##  V Summary and Conclusions

In the current evaluation of a reactive controller for crowd navigation, we
have shown its feasible to navigate around natural pedestrian crowds
demonstrating the controller’s compatibility with the behaviour of bystander
pedestrians. During all 110 trials in different crowd types: sparse, flows,
and mixed crowds, we did not report any incidents with pedestrians, although,
we recorded 7 contacts within safe limits. On the contrary, any comments made
by bystanders were positive about the perceived utility of assistive
navigation technology for mobility-impaired users.

The reactive navigation controllers showed to perform similarly in trajectory
efficiency and fluency during the motions. The controllers differ in the way
to avoid obstacles, one mainly deviating through angular velocity (MDS) and
the other mostly by reducing speed (RDS). It is hence expected that they lead
to measurable differences such as faster displacement to the goal for MDS than
RDS (confirmed by results), larger jerkiness for MDS than RDS (confirmed by
data). One may have expected that MDS leads to more collisions than RDS but
this is not confirmed by the results. However, the number of collisions were
too few to conclude.

A significant difference found in the reactive controller response was given
by the type of crowd, where sparse and mixed traffic crowds in the medium
density showed to be more difficult to navigate with a lower agreement with
the user at the same level of contribution. In contrast, flow crowds made it
easier for the controller to respond to a somewhat uniform response of the
surrounding pedestrians, as validated with the equal agreement levels (see,
Fig. 7).

The proposed reactive controller focused on a single layer of the robot
response for crowd navigation which performs efficiently compared with shared
control, with no significant difference in time to goal, minimal distance to
pedestrians, motion jerk, and path length. Further work should focus on
situational awareness of the complex mixed crowd environments, as required
when interacting in complex environments, e.g., on some occasions, the robot
did not interact appropriately with the environment. i.e., getting inside
stores, getting in the middle of people lining up, and entering shops in the
wrong direction. In comparison, shared control was more natural, and most of
the tests ran smoothly without bystanders even noticing the robot.

## Acknowledgement

The experiments were approved with an ethical protocol by the human research
ethical committee of EPFL (Approval No: HREC-032-2019), and with the approval
and cooperation of the office for mobility, the police and the office for
parks, and the public domain of the city of Lausanne. Approval numbers: 395128
and 416008. Disclaimer: D.P. and K.S. hold the patents of the robot Qolo and
shares in the company Qolo Inc.

## References

  * [1] P. Trautman and K. Patel, “Real time crowd navigation from first principles of probability theory,” _Proceedings of the International Conference on Automated Planning and Scheduling_ , vol. 30, no. 1, pp. 459–467, Jun. 2020.
  * [2] A. J. Sathyamoorthy, J. Liang, U. Patel, T. Guan, R. Chandra, and D. Manocha, “Densecavoid: Real-time navigation in dense crowds using anticipatory behaviors,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 11 345–11 352.
  * [3] P. Salvini, D. Paez-Granados, and A. Billard, “On the Safety of Mobile Robots Serving in Public Spaces: Identifying gaps in en ISO 13482:2014 and calling for a new standard,” _ACM Transactions on Human-Robot Interaction_ , vol. 10, no. 3, pp. 1–29, 2021. [Online]. Available: https://doi.org/10.1145/3442678
  * [4] ——, “Safety Concerns Emerging from Robots Navigating in Crowded Pedestrian Areas,” _International Journal of Social Robotics (SORO)_ , vol. 14, no. 2, pp. 441–462, 2022. [Online]. Available: https://doi.org/10.1007/s12369-021-00796-4
  * [5] B. Yi, S. Gottschling, J. Ferdinand, N. Simm, F. Bonarens, and C. Stiller, “Real time integrated vehicle dynamics control and trajectory planning with mpc for critical maneuvers,” in _2016 IEEE Intelligent Vehicles Symposium (IV)_ , 2016, pp. 584–589.
  * [6] Y. Chen, F. Zhao, and Y. Lou, “Interactive model predictive control for robot navigation in dense crowds,” _IEEE Transactions on Systems, Man, and Cybernetics: Systems_ , pp. 1–13, 2021.
  * [7] H. Bai, S. Cai, N. Ye, D. Hsu, and W. S. Lee, “Intention-aware online pomdp planning for autonomous driving in a crowd,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 454–460.
  * [8] Y. Luo, P. Cai, A. Bera, D. Hsu, W. S. Lee, and D. Manocha, “Porca: Modeling and planning for autonomous driving among many pedestrians,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 4, pp. 3418–3425, 2018.
  * [9] P. Cai, Y. Luo, D. Hsu, and W. S. Lee, “Hyp-despot: A hybrid parallel algorithm for online planning under uncertainty,” _The International Journal of Robotics Research_ , vol. 40, no. 2-3, pp. 558–573, 2021.
  * [10] A. J. Sathyamoorthy, U. Patel, T. Guan, and D. Manocha, “Frozone: Freezing-free, pedestrian-friendly navigation in human crowds,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 3, pp. 4352–4359, 2020.
  * [11] Y. Kobayashi, T. Sugimoto, K. Tanaka, Y. Shimomura, F. J. Arjonilla Garcia, C. H. Kim, H. Yabushita, and T. Toda, “Robot Navigation Based on Predicting of Human Interaction and its Reproducible Evaluation in a Densely Crowded Environment,” _International Journal of Social Robotics_ , 2021.
  * [12] L. Huber, A. Billard, and J.-J. Slotine, “Avoidance of Convex and Concave Obstacles With Convergence Ensured Through Contraction,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 1462–1469, 2019. [Online]. Available: https://doi.org/10.1109/lra.2019.2893676
  * [13] D. Paez-Granados, V. Gupta, and A. Billard, “Unfreezing social navigation : Dynamical systems based compliance for contact control in robot navigation,” in _IEEE International Conference on Robotics and Automation (ICRA-2022)_ , vol. 1, 5 2022, pp. 8368–8374. [Online]. Available: https://doi.org/10.1109/ICRA46639.2022.9811772
  * [14] C. Chen, Y. Liu, S. Kreiss, and A. Alahi, “Crowd-robot interaction: Crowd-aware robot navigation with attention-based deep reinforcement learning,” in _IEEE International Conference on Robotics and Automation_ , 2019.
  * [15] F. Grzeskowiak, D. Gonon, D. Dugas, D. Paez-Granados, J. Chung, J. Nieto, R. Siegwart, A. Billard, M. Babel, and J. Pettré, “Crowd against the machine: A simulation-based benchmark tool to evaluate and compare robot capabilities to navigate a human crowd,” in _IEEE International Conference on Robotics and Automation (ICRA-2021)_ , 2021. [Online]. Available: https://doi.org/10.1109/ICRA48506.2021.9561694
  * [16] D. Paez-Granados, H. Kadone, M. Hassan, Y. Chen, and K. Suzuki, “Personal mobility with synchronous trunk-knee passive exoskeleton: Optimizing human-robot energy transfer,” _IEEE/ASME Transactions on Mechatronics_ , vol. 1, pp. 1–12, 2022. [Online]. Available: https://doi.org/10.1109/TMECH.2021.3135453
  * [17] D. Paez-Granados, Y. He, D. Gonon, L. Huber, and A. Billard, “3d point cloud and rgbd of pedestrians in robot crowd navigation: Detection and tracking,” _IEEE DataPort_ , 12 2021. [Online]. Available: https://doi.org/10.21227/ak77-d722
  * [18] D. Paez-Granados and A. Billard, “Crash test-based assessment of injury risks for adults and children when colliding with personal mobility devices and service robots,” _Nature Scientific Reports_ , vol. 12, pp. 1–13, 3 2022\. [Online]. Available: https://doi.org/10.1038/s41598-022-09349-9
  * [19] Y. Chen, D. Paez-Granados, H. Kadone, and K. Suzuki, “Control interface for hands-free navigation of standing mobility vehicles based on upper-body natural movements,” in _IEEE International Conference on Intelligent Robots and Systems_. IEEE, 2020, pp. 11 322–11 329. [Online]. Available: https://doi.org/10.1109/IROS45743.2020.9340875
  * [20] D. J. Gonon, D. Paez-Granados, and A. Billard, “Reactive Navigation in Crowds for Non-holonomic Robots with Convex Bounding Shape,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 4728–4735, 2021. [Online]. Available: https://doi.org/10.1109/LRA.2021.3068660
  * [21] D. Jia, A. Hermans, and B. Leibe, “DR-SPAAM: A Spatial-Attention and Auto-regressive Model for Person Detection in 2D Range Data,” in _International Conference on Intelligent Robots and Systems (IROS)_ , 2020\.
  * [22] G. Jocher, Y. Kwon, guigarfr, perry0418, J. Veitch-Michaelis, Ttayu, D. Suess, F. Baltacı, G. Bianconi, IlyaOvodov, Marc, e96031413, C. Lee, D. Kendall, Falak, F. Reveriano, FuLin, GoogleWiki, J. Nataprawira, J. Hu, LinCoce, LukeAI, NanoCode012, NirZarrabi, O. Reda, P. Skalski, SergioSanchezMontesUAM, S. Song, T. Havlik, and T. M. Shead, “ultralytics/yolov3: v9.5.0 - YOLOv5 v5.0 release compatibility update for YOLOv3,” Apr. 2021. [Online]. Available: https://doi.org/10.5281/zenodo.4681234
  * [23] D. Paez-Granados and V. Gupta, “ROS-based Qolo’s controller architecture for shared control and crowd navigation,” 3 2022. [Online]. Available: https://github.com/DrDiegoPaez/qolo_ros/
  * [24] D. Jia and B. Leibe, “Person-MinkUNet: 3D Person Detection with LiDAR Point Cloud,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition Workshops (CVPRW)_ , 2021.
  * [25] X. Weng, J. Wang, D. Held, and K. Kitani, “3D Multi-Object Tracking: A Baseline and New Evaluation Metrics,” _IROS_ , 2020.
  * [26] T. Carlson and Y. Demiris, “Collaborative control for a robotic wheelchair: Evaluation of performance, attention, and workload,” _IEEE Transactions on Systems, Man, and Cybernetics, Part B: Cybernetics_ , vol. 42, pp. 876–888, 2012.
  * [27] D. Gopinath, S. Jain, and B. D. Argall, “Human-in-the-loop optimization of shared autonomy in assistive robotics,” _IEEE Robotics and Automation Letters_ , vol. 2, pp. 247–254, 2016.
  * [28] A. Erdogan and B. D. Argall, “The effect of robotic wheelchair control paradigm and interface on user performance, effort and preference: An experimental assessment,” _Robotics and Autonomous Systems_ , vol. 94, pp. 282–297, 2017.