# Cooperative Sensing with Impurities in a Two-Dimensional Subwavelength Array

Oliver August Dall’Alba Sandberg<EMAIL_ADDRESS>Department of
Physics, Harvard University, Cambridge, Massachusetts 02138, USA  Center for
Hybrid Quantum Networks (Hy-Q), The Niels Bohr Institute, University of
Copenhagen, DK-2100 Copenhagen Ø, Denmark  Stefan Ostermann  Department of
Physics, Harvard University, Cambridge, Massachusetts 02138, USA  Susanne F.
Yelin  Department of Physics, Harvard University, Cambridge, Massachusetts
02138, USA

###### Abstract

We propose a versatile quantum sensing protocol based on two dissipatively
coupled distant atoms embedded as impurities in a two-dimensional sub-
wavelength atomic array. The array acts as a waveguide for the emitter light,
creating cooperative enhancement that allows for more efficient population
transfer. By monitoring the population of one of the impurity atoms, it is
possible to detect frequency shifts in the emitters’ resonance frequencies. We
analytically estimate achievable sensitivities as well as the dependence on
various system parameters. The proposed protocol is robust against various
environmental factors and perturbations, which enhances its applicability in
real-world scenarios.

Figure 1: A pair of impurities (blue) placed in a sub-wavelength atomic
lattice with spacing $a\lesssim\lambda$. In our protocol, we locally excite
one atom and detect the population at the other. The lattice atoms couple via
optical dipole-dipole interactions and serve as a Markovian bath mediating the
interactions and modifying the bare decay rates. We do not invoke the
assumption of independent atomic emission, instead the emitted fields of the
lattice atoms interfere with each other, giving rise to a coherent collective
enhancement to the coupling of the impurities.

Efficient light-matter interfaces will be a vital advancement enabling future
quantum technologies [1]. However, establishing strong coupling between light
and matter is challenging due to the small interaction cross-section between
matter and single photons, which is in general proportional to the photon’s
wavelength squared [2]. To overcome this challenge, various platforms which
alter the radiative environment by introducing dielectric media near quantum
emitters have been developed over the past decade [3]. Besides enhancing the
coupling between light and matter on the single photon level, these setups
enable long-range couplings between distant emitters and modify emitter decay
rates.

Prominent examples of such platforms are quantum emitters, such as quantum
dots [4], nanoparticles [5, 6], or atoms coupled to cavities [7, 8],
nanophotonic waveguides [9], photonic band-gap materials [10], or other
dielectric structures, like atomic lattices with sub-wavelength spacing [11,
12, 13, 14, 15, 16, 17]. These platforms offer a wide range of applications,
including the generation of super- and subradiant states [18, 21, 22, 23, 19,
20]; non-classical states of light [24, 25, 26, 27]; quantum simulation and
information processing [28]; and sensing [19, 29].

In this Letter, we propose a versatile measurement protocol to exploit two
quantum emitters, dissipatively coupled through their embedding in a 2D atomic
array [31, 30, 32], for sensing applications. We introduce the term _array
QED_ to denote this conceptual framework, drawing parallels to concepts from
cavity [33]\- and waveguide [34] QED. The utilization of quantum platforms for
quantum sensing capitalizes on a core feature of quantum systems: their
susceptibility to external disturbances. While these effects are detrimental
to quantum cryptography and quantum computing, they present an opportunity for
highly sensitive measurements of electric and magnetic fields [35], time and
frequency [36], rotations [37], temperature, and pressure [38, 39].

In our approach, we measure the population transfer between two strongly-
coupled distant emitters, exploiting their separation to perform local
excitation at one emitter and local population readout at the other. In
analogy to waveguide QED and cavity QED, array QED can benefit from collective
enhancement, leading to much stronger atom-photon interactions compared to
free space. We show that this enhancement enables the measurement of minute
frequency shifts.

Through an analytic description of the population transfer dynamics, we show
how this cooperative enhancement can be used for sensing, while remaining
robust to certain types of noise.

_Model._ —A pair of cooperatively coupled quantum emitters, labelled $1$ and
$2$, are embedded in a 2D sub-wavelength atomic array as shown in Fig. 1a.
These emitters have decay rates $\gamma_{1,2}$ and resonance frequencies
$\omega_{1,2}=2\pi c/\lambda_{0}^{1,2}$, where $\lambda_{0}^{1,2}$ is the
transition wavelength of emitters $1$ and $2$. The lattice is a large two-
dimensional atomic array with a sub-wavelength lattice spacing
$a<\lambda_{0}^{1,2}$. In this setup, the atoms interact through light-induced
dipole-dipole interactions.

After adiabatically eliminating the lattice, the system can be modeled by the
effective non-Hermitian Hamiltonian:

$\displaystyle\mathcal{H}_{\mathrm{eff}}$
$\displaystyle=\left(\begin{array}[]{cc}\Omega_{1}-i\Gamma_{1}/2&\sqrt{\gamma_{1}\gamma_{2}}\kappa\\\
\sqrt{\gamma_{1}\gamma_{2}}\kappa&\Omega_{q}-i\Gamma_{2}/2\\\
\end{array}\right),$ (3)

which is an effective system comprised of only the two impurities (see Fig.
1b) realized by collective coupling induced by the lattice. The coherent and
dissipative interaction strengths between the atoms are determined via the
real and imaginary part of the Green’s tensor for a point dipole in free space
(see Supplementary Information [40]), physically corresponding to a
modification of the effective frequencies
$\Omega_{1}\equiv\gamma_{1}\mathrm{Re}\\{\Sigma\\},\Omega_{2}\equiv\Delta+\gamma_{2}\mathrm{Re}\\{\Sigma\\}$,
and effective decay rates
$\Gamma_{1}\equiv\gamma_{1}\left(1-2\mathrm{Im}\\{\Sigma\\}\right)=\gamma_{1}\Gamma_{\mathrm{coop}}$
and
$\Gamma_{2}\equiv\gamma_{2}\left(1-2\mathrm{Im}\\{\Sigma\\}\right)=\gamma_{2}\Gamma_{\mathrm{coop}}$,
where $\Delta=\omega_{1}-\omega_{2}$ is the detuning between the two
impurities. The parameter

$\displaystyle\Gamma_{\mathrm{coop}}\equiv 1-2\mathrm{Im}(\Sigma),$ (4)

is the cooperativity factor in array QED that describes the lengthening of the
excited state lifetimes for the impurities ($0<\Gamma_{\mathrm{coop}}<1)$, and
is thus a key parameter in our protocol, allowing the system to retain
coherence for longer. The self-energy $\Sigma$ describes the modification to
the atom’s energy as a result of its own field and the fields of the
surrounding atoms, while the coupling strength $\kappa$ describes the strength
of interaction between neighboring atoms. Note that energy shifts resulting in
$\Sigma$ usually diminish the sensitivity of lattice based sensors [41]. The
protocol presented below is robust against these cooperatively induced shifts.
In general, the system dynamics of a dissipative quantum system should be
modelled in the master equation formalism. However, in the single-excitation
subspace, and without external driving, the quantum jump terms in the master
equation can be neglected [42] and the system dynamics are fully described by
the effective Hamiltonian (3).

It may appear that this Hamiltonian produces standard damped Rabi oscillations
(see also Fig. 2a). However, by taking into account both dissipative and
cooperative coupling, the effective Hamiltonian acquires additional imaginary
components, extending the physics beyond the conventional damped Rabi regime.

_Sensing protocol_.—The key idea of the proposed protocol is illustrated in
Fig. 2a. After an initial excitation of impurity $2$, the population in
impurity $1$ is detected after some time $t_{0}$. The population in the
detector atom $1$ is highly sensitive to small changes in the relative
resonance frequencies between the quantum emitters, described by the detuning
$\tilde{\Delta}=\Delta/\sqrt{\gamma_{1}\gamma_{2}}$ (see Fig. 2a,b.).
Throughout this Letter, we will use the tilde ($\sim$) to denote a
renormalization of a parameter by the factor $\sqrt{\gamma_{1}\gamma_{2}}$,
where $\gamma_{i}$ has units of frequency. As a result of this sensitivity, an
external disturbance to impurity $2$ that results in a shift of the atomic
resonance frequency, e.g., a magnetic or gravitational field, can be detected
in the dynamics of impurity $1$. This is the key feature of our proposed
sensing protocol, since it allows the measurement of frequency shifts via
population measurements in impurity $1$.

Figure 2: a) The population of the initally un-excited impurity $1$ as a
function of the time $\tilde{t}\Gamma_{\mathrm{coop}}$, where
$\tilde{t}=\sqrt{\gamma_{1}\gamma_{2}}t$ [see Eq. (6)] for a $10\times 10$
lattice, with lattice spacing $a=0.3\lambda_{0}^{s}$ and impurity distance
$d=2a$. The blue curve corresponds to maximal population transfer at
$\tilde{\Delta}=0$. A small change in detuning ($\tilde{\Delta}=0.1$)
diminishes the achievable population transfer (red curve). The dashed lines
show the overall decay of the population (proportional to
$\Gamma_{\mathrm{coop}}$) without the transition between $1$ and $2$. b) The
maximum population as a function of detuning for the same parameters as a)
above. In the inset, the population is shown for values of
$R=\gamma_{1}/\gamma_{2}$ from left to right $R=(1/30,1/10,1,10,30)$. The
height and width of the quasi-Lorenzian
$|c_{1_{\mathrm{max}}}|^{2}(\Delta)\approx\gamma_{1}\gamma_{2}\left|\frac{\kappa}{S}\right|^{2}e^{-\frac{\pi\bar{\gamma}\Gamma_{\mathrm{coop}}}{2\mathrm{Re}(S)}}$
(dashed lines) lowers and widens as we move away from the ratio
$R=\gamma_{1}/\gamma_{2}=1$.

The system dynamics are governed by the differential equations
$(\dot{c}_{1},\dot{c}_{2})^{T}=\mathcal{H}_{\mathrm{eff}}\cdot(s,q)^{T}$, the
solutions to which are superpositions of exponentials of the eigenvalues of
$H_{\mathrm{eff}}$, which are
$\omega_{\pm}=\bar{\Omega}-i\frac{\bar{\gamma}\Gamma_{\mathrm{coop}}}{2}\pm S$
with the quasi-Rabi frequency

$\displaystyle
S(\Delta)=\sqrt{\frac{1}{4}\left(\Delta_{0}-\Delta-i\frac{\Gamma_{\mathrm{coop}}}{2}(\gamma_{1}-\gamma_{2})\right)^{2}+\gamma_{1}\gamma_{2}\kappa^{2}}.$
(5)

The resonant detuning
$\Delta_{0}=\mathrm{Re}(\Sigma)\left(\gamma_{1}-\gamma_{2}\right)$,
corresponds physically to when the two impurity frequencies are equal
$\Omega_{1}=\Omega_{2}$. In the inset of Figure 2b, we can see that the
resonance peaks shift as the ratio $R=\gamma_{1}/\gamma_{2}$ is changed. The
location of the maximum of these peaks is given by $\Delta_{0}$. However,
since operation with $\gamma_{1}=\gamma_{2}$ is globally optimal (see
Supplementary Information III), both the resonant detuning $\Delta_{0}$ and
the term containing $\Gamma_{\mathrm{coop}}$ drop out of the quasi-Rabi
frequency (5).

As stated before, after one impurity is excited, the population of the other
impurity is strongly dependent on the detuning $\Delta$ between their
frequencies. Assuming impurity $2$ is initially excited, the population of
impurity $1$ at time $t$ is given as

$\displaystyle|c_{1}|^{2}$
$\displaystyle=\frac{\gamma_{1}\gamma_{2}|\kappa|^{2}}{2|S|^{2}}e^{-t\bar{\gamma}\Gamma_{\mathrm{coop}}}\left[\cosh(2t\mathrm{Im}(S))-\cos(2t\mathrm{Re}(S))\right],$
(6)

where $S=S(\Delta)$ shows where this dependence on detuning enters the
dynamics. $\bar{\gamma}=(\gamma_{1}+\gamma_{2})/2$ is the mean decay rate.
Since the cosh term diverges for $t\to\infty$, we require
$\bar{\gamma}\Gamma_{\mathrm{coop}}>2\mathrm{Im}(S)$ (see Supplementary
Information I). As we will later discuss, the cosh term and the decay term
containing $\Gamma_{\textrm{coop}}$ are the key terms that lengthen the
system’s coherence time and allows for enhanced sensitivity.

The dynamics of $c_{1}(t)$ differ from the well known behavior of laser driven
atoms performing Rabi oscillations. In our system, $S(\Delta)$ takes on the
analogous role of the Rabi frequency, with the key difference being that both
the quasi-detuning
$\Delta_{\mathrm{quasi}}=\Delta-\Delta_{0}-i\frac{\Gamma_{\mathrm{coop}}}{2}(\gamma_{2}-\gamma_{1})$
given by the difference between the diagonal elements of
$\mathcal{H}_{\mathrm{eff}}$, and consequently the quasi-Rabi frequency
$S(\Delta)$ have imaginary components. These imaginary components are a
consequence of the decay channels available due to the cooperative effects
induced via the surrounding lattice atoms.

Due to the non-Hermitian effective Hamiltonian (3) the eigenvalue spectrum of
$H$ admits exceptional points [44, 43, 45], which have been previously
investigated as candidates for quantum sensing protocols [46, 47]. For the
protocol presented below, the exceptional points are not optimal points for
sensing, so are not the focus of this work (see Supplementary Information II).

We will now precisely define how we quantify the sensitivity of the protocol.
Since the protocol requires the detection of single photons at the location of
impurity $1$, each measurement of the population in impurity $1$ is a
Bernoulli trial (the detector clicks or it doesn’t) where the probability of
success is binomially distributed with probability $p=|c_{1}(t_{0})|^{2}$,
i.e., we take samples from the response curve in Figure 2b. From these
measurements, one can estimate the quantity $|c_{\mathrm{signal}}|^{2}$, which
is the change in the population due to some additional detuning
$\Delta_{\mathrm{signal}}$. Since the population changes with
$\Delta_{\mathrm{signal}}$, by measuring $|c_{1}|^{2}$, we can obtain an
estimate of $\Delta_{\mathrm{signal}}$ from $|c_{\mathrm{signal}}|^{2}$.

Since $\Delta_{\mathrm{signal}}$ inherits the binomial distribution from
$|c_{\mathrm{signal}}|^{2}$, we can propagate the measurement uncertainty to
obtain an estimate (see Supplementary Information V for more details)

$\displaystyle\sigma_{\Delta_{\mathrm{signal}}}=\frac{\sigma_{|c_{1}|^{2}}}{\left|\frac{\partial|c_{1}|^{2}}{\partial\Delta}\right|},$
(7)

where the derivative is evaluated at an added detuning
$\Delta_{\mathrm{add}}$, which is an additional degree of freedom that allows
for further optimization of the system. Since external disturbances will alter
the optimal operating point, the additional control afforded by
$\Delta_{\mathrm{add}}$ gives the protocol additional flexibility to operate
at the new optimum. The measurements of the population $|c_{1}|^{2}$ are
distributed binomially and so have the well-known variance
$\sigma^{2}=p(1-p)$, where $p=|c_{1}|^{2}$. A large derivative
$\partial|c_{1}|^{2}/\partial\Delta$ means a small change in
$\Delta_{\mathrm{signal}}$ leads to a large change in the $|c_{1}|^{2}$,
reducing the uncertainty in $\Delta_{\mathrm{signal}}$ for a given uncertainty
in $p$. The derivative encodes the intuitive physical understanding that the
‘narrowness’ of Fig. 2b leads to a more sensitive system. Additionally, the
population variance $\sigma_{|c_{1}|^{2}}$ is minimized at $p=1$, so given two
systems where the derivative is equal, it is advantageous to choose system
parameters that lead to the largest population transfer. However, any change
in the population itself is likely to also affect the behavior with respect to
$\Delta$, and thus to fully optimize the sensing, we need to consider the
interplay between $p$ and this derivative.

To summarize, the observable of interest is the signal detuning
$\Delta_{\mathrm{signal}}$. For a certain frequency shift, which is generated
via an external _local_ perturbation (e.g., a magnetic field at the location
of impurity $2$). Since the system is highly sensitive to changes in $\Delta$,
the protocol is able to detect small external disturbances
$\Delta_{\mathrm{signal}}$. We quantify this sensitivity by computing the
uncertainty $\sigma_{\Delta_{\mathrm{signal}}}$ [see 2b and (7)]. We will
henceforth drop the subscript $\Delta_{\mathrm{signal}}$ and refer to the
uncertainty simply as $\sigma$, the standard deviation of a measurement of
$\Delta_{\mathrm{signal}}$. A question now remains: how do our system
parameters, the lattice spacing $a$, the added detuning
$\Delta_{\mathrm{add}}$ and the measurement time $t_{0}$ affect this
measurement sensitivity?

Figure 3:  (a) The cooperative enhancement factor $\Gamma_{\mathrm{coop}}$, as
well as the real and twice the imaginary part of the quasi-Rabi frequency as a
function of the lattice spacing $\tilde{a}$. (b) Standard deviation of
measurements of $\Delta_{\mathrm{signal}}$ as a function of the measurement
time $\tilde{t}_{0}\Gamma_{\mathrm{coop}}$, and
$\tilde{\Delta}_{\mathrm{add}}$ where
$\tilde{t}_{0}=\sqrt{\gamma_{1}\gamma_{2}}t_{0}$ and
$\tilde{\Delta}_{\mathrm{add}}=\Delta_{\mathrm{add}}/\sqrt{\gamma_{1}\gamma_{2}}$.
The resonances are a result of the derivative reaching an inflection point,
leading to a divergence in Eq. (7). We set $\gamma_{1}=\gamma_{2}$ for this
plot as it represents the universally optimal value for this protocol (see
Supplementary Information III).

The lattice spacing changes the timescale of the dynamics, where a smaller
lattice spacing means faster population exchange between the impurities. This
makes physical sense: the smaller the lattice spacing, the smaller the
physical distance the excitation needs to traverse, and assuming equal decay
rates and propagation velocity, the population will oscillate more rapidly.
This is reflected in the real part of the quasi-Rabi frequency, which we show
in Fig. 3a. As $a$ deceases, $\mathrm{Re}(\tilde{S})$ increases monotonically,
leading to a corresponding increase in the oscillation frequency, in analogy
with Rabi oscillations. An enhanced oscillation rate results in a larger
change in the population given some change to the detuning, $\Delta$, thus
improving the sensitivity.

There are two decay terms in (6): one cosh term containing $\mathrm{Im}(S)$
and one containing $\Gamma_{\mathrm{coop}}$. To have the best sensitivity, the
system should retain coherence as long as possible, i.e., we would like the
overall decay to be as small as possible. Since the cosh term contributes to
an overall growth of the population for $\mathrm{Im}(S)>0$, it is optimal to
satisfy the condition $\Gamma_{\mathrm{coop}}>2\mathrm{Im}(\tilde{S})$ as
close to equality as possible. However, the overall decay is dominated by
$\Gamma_{\mathrm{coop}}$, which should also be minimized. The optimum for both
conditions lies at $\tilde{a}\sim 0.12$ (see Fig. 3a), which is a global
optimum for the sensitivity with respect to the lattice spacing. Generally,
decreasing the lattice spacing has the dual effect of increasing the
oscillation frequency of the dynamics themselves, whilst decreasing the decay,
so overall the system oscillates more rapidly and does so for longer, both of
which improve the sensitivity of the protocol.

In Fig. 3b, $\sigma$ displays alternating broad and narrow resonances as a
function of the measurement time $t_{0}$. The broad resonances in this plot
stems from the population hitting a maximum, leading to a zero derivative and
a subsequent divergence in $\sigma$. The narrower resonances correspond to the
population reaching a minimum. For a given $t_{0}$, the added detuning
$\Delta_{\mathrm{add}}$ serves to move these resonances around, resulting in
the repeating structures that can be seen in Fig. 3b. An added detuning
$\Delta_{\mathrm{add}}$ increases the effective oscillation frequency, and
thus will tend to increase the derivative
$\partial(|c_{1}|^{2})/\partial\Delta$. However, a detuning leads to decreased
coupling efficiency, reducing the population and increasing the variance. The
optimal choice for $\Delta_{\mathrm{add}}$ lies in the dark horizontal bands
Fig. 3b, avoiding the resonances in $t_{0}$. Physically, this corresponds to
choosing $\Delta_{\mathrm{add}}$ and $t_{0}$ such that the population is
changing rapidly with $\Delta$, maximizing the derivative
$\partial|c_{1}|^{2}/\partial\Delta$. The possible choices of $t_{0}$ are
expanded greatly due to the decay suppressing effects of
$\Gamma_{\mathrm{coop}}$ and $\mathrm{Im}(S)$, as the population can remain
high even after several cycles of oscillations. Finally, we note that while
the discussion above focused on initial excitation of a single impurity, the
protocol can also be applied to alternative initializations by tracking the
population exchange between the two impurities over time.

_Robustness to noise._ — In realistic experiments, the lattice will have
imperfections due to both atomic motion and position. We model these
imperfections by randomly sampling the positions of the lattice atoms from a
Gaussian distribution centered about the old position but with a variance that
we call “position disorder” (this is to prevent confusion with the
$\sigma_{\Delta_{\mathrm{signal}}}$ already in use). This procedure breaks the
lattice symmetry and requires that we include different self-energies
$\Sigma_{1}$ and $\Sigma_{2}$, for each impurity. Now
$\Gamma_{\mathrm{coop}}=1-\mathrm{Im}(\Sigma_{1})-\mathrm{Im}(\Sigma_{2})$ and
$\Delta_{0}=\gamma_{1}\mathrm{Re}(\Sigma_{1})-\gamma_{2}\mathrm{Re}(\Sigma_{2})$.
In Fig. 4a), we see that as position disorder grows, $\Gamma_{\mathrm{coop}}$
grows along with it, and as is reflected in Fig. 4b), so does the sensitivity
of the protocol, which follows the variations in $\Gamma_{\mathrm{coop}}$
closely. As before, it is the combination of a small $\Gamma_{\mathrm{coop}}$
that is close to (but larger than) a small value of $2\mathrm{Im}(\tilde{S})$
that will lead to optimal results for the sensing protocol. The real part of
the quasi-Rabi frequency stays roughly constant throughout the range of
position disorders, indicating that the oscillation rate of the population
transfer is not strongly affected by position disorder. In Fig. 4b), we show
the effect of the position disorder on $\sigma$. Changes in the lattice can be
detected by the measurements of the population, giving the flexibility to find
a new optimal operating point. Since $\Gamma_{\mathrm{coop}}$ stays relatively
small even for large position disorder, the performance of the re-optimized
protocol significantly outperforms the case of two impurities without a
lattice (shown by the dashed line), for all values of the position disorder.
This illustrates that the protocol presented above is robust against lattice
disorder and cooperative enhancement prevails in this case.

Figure 4:  a) The modified
$\Gamma_{\mathrm{coop}}=1-\mathrm{Im}(\Sigma_{1})-\mathrm{Im}(\Sigma_{2})$ as
well as the real and imaginary parts of the quasi-Rabi frequency
$\tilde{S}=S/\sqrt{\gamma_{1}\gamma_{2}}$. The dashed blue lines represent the
negative values of $2\mathrm{Im}(\tilde{S})$. b) The measurement standard
deviation $\sigma$ plotted for a lattice initialized with amount of Gaussian
disorder. We show $\sigma$ when the optimal parameters at zero disorder are
unchanged (Standard), as well as the re-optimized curve, which involves taking
into account the new lattice to find a new set of optimal parameters at each
value of the disorder.

_Conclusions and Outlook._ — We introduced a quantum sensing protocol based on
coherent photon exchange between distant quantum emitters. We showed that
local population measurements can be used to detect frequency shifts due to
the strong sensitivity of the photon transfer efficiency on the relative
detuning between the emitters. The population transfer is strongly enhanced by
the collective effects present in the system. This is in contrast to other
lattice based metrology applications, which are in general harmed by such
effects via dephasing and cooperative shifts. While the discussion in this
Letter was based on an implementation with impurities embedded in a
cooperative array, the protocol is, in principle, generalizable to related
platforms (waveguide coupled quantum dots, photonic bandgap materials, solid
state based implementations, NV centers etc).

The introduced approach opens up exciting avenues for cooperatively enhanced
quantum sensing. The major challenge to overcome is local single photon
readout (which is becoming feasible in, e.g., modern optical tweezer based
setups).

Additionally, the population dynamics of the two impurities could be used to
monitor the lattice dynamics, forming the basis for a new tool for device
characterization. To further improve the sensitivity, another potential future
direction is to extend the study to systems beyond the single excitation
regime, where strong photon non-linearities will occur.

###### Acknowledgements.

We would like to thank E. Kalberer for assistance in the production of
figures. O.S. acknowledges funding from the European Union’s Horizon 2020
research and innovation program under the Marie Skłodowska-Curie grant
agreement No. 801199. SFY acknowledges funding from the NSF via PHY-2207972,
the CUA PFC PHY-2317134, and the Q-SEnSE QLCI OMA-2016244. We gratefully
acknowledge discussions with Anders S. Sørensen.

## References

  * Hammerer  _et al._ [2010] K. Hammerer, A. S. Sørensen, and E. S. Polzik, Reviews of Modern Physics 82, 1041 (2010).
  * Gutzler  _et al._ [2021] R. Gutzler, M. Garg, C. R. Ast, K. Kuhnke, and K. Kern, Nature Reviews Physics 3, 441 (2021), number: 6 Publisher: Nature Publishing Group.
  * Mirhosseini  _et al._ [2019] M. Mirhosseini, E. Kim, X. Zhang, A. Sipahigil, P. B. Dieterle, A. J. Keller, A. Asenjo-Garcia, D. E. Chang, and O. Painter, Nature 569, 692 (2019), publisher: Nature Publishing Group.
  * Lodahl  _et al._ [2015] P. Lodahl, S. Mahmoodian, and S. Stobbe, Reviews of Modern Physics 87, 347 (2015).
  * Gonzalez-Ballestero  _et al._ [2021] C. Gonzalez-Ballestero, M. Aspelmeyer, L. Novotny, R. Quidant, and O. Romero-Isart, Science 374, eabg3027 (2021), publisher: American Association for the Advancement of Science.
  * Rieser  _et al._ [2022] J. Rieser, M. A. Ciampini, H. Rudolph, N. Kiesel, K. Hornberger, B. A. Stickler, M. Aspelmeyer, and U. Delić, Science 377, 987 (2022), publisher: American Association for the Advancement of Science.
  * Rempe [1993] G. Rempe, Contemporary Physics 34, 119 (1993), publisher: Taylor & Francis _eprint: https://doi.org/10.1080/00107519308213809.
  * Westmoreland  _et al._ [2019] D. E. Westmoreland, K. P. McClelland, K. A. Perez, J. C. Schwabacher, Z. Zhang, and E. A. Weiss, The Journal of Chemical Physics 151, 210901 (2019).
  * Vetsch  _et al._ [2010] E. Vetsch, D. Reitz, G. Sagué, R. Schmidt, S. T. Dawkins, and A. Rauschenbeutel, Physical Review Letters 104, 203603 (2010).
  * Hood  _et al._ [2016] J. D. Hood, A. Goban, A. Asenjo-Garcia, M. Lu, S.-P. Yu, D. E. Chang, and H. J. Kimble, Proceedings of the National Academy of Sciences 113, 10507 (2016), publisher: Proceedings of the National Academy of Sciences.
  * Bettles  _et al._ [2016] R. J. Bettles, S. A. Gardiner, and C. S. Adams, Physical Review Letters 116, 103602 (2016).
  * Shahmoon  _et al._ [2017] E. Shahmoon, D. S. Wild, M. D. Lukin, and S. F. Yelin, Physical Review Letters 118, 113601 (2017).
  * Asenjo-Garcia  _et al._ [2017] A. Asenjo-Garcia, M. Moreno-Cardoner, A. Albrecht, H. Kimble, and D. Chang, Physical Review X 7, 031024 (2017).
  * Rui  _et al._ [2020] J. Rui, D. Wei, A. Rubio-Abadal, S. Hollerith, J. Zeiher, D. M. Stamper-Kurn, C. Gross, and I. Bloch, Nature 583, 369 (2020).
  * Srakaew  _et al._ [2023] K. Srakaew, P. Weckesser, S. Hollerith, D. Wei, D. Adler, I. Bloch, and J. Zeiher, Nature Physics 19, 714 (2023).
  * Reitz  _et al._ [2022] M. Reitz, C. Sommer, and C. Genes, PRX Quantum 3, 010201 (2022), publisher: American Physical Society.
  * Ruostekoski [2023] J. Ruostekoski, Physical Review A 108, 030101 (2023), publisher: American Physical Society.
  * Masson  _et al._ [2020] S. J. Masson, I. Ferrier-Barbut, L. A. Orozco, A. Browaeys, and A. Asenjo-Garcia, Physical Review Letters 125, 263601 (2020).
  * Rubies-Bigorda  _et al._ [2023] O. Rubies-Bigorda, S. Ostermann, and S. F. Yelin, Physical Review A 107, L051701 (2023).
  * Tiranov  _et al._ [2023] A. Tiranov, V. Angelopoulou, C. J. van Diepen, B. Schrinski, O. A. D. Sandberg, Y. Wang, L. Midolo, S. Scholz, A. D. Wieck, A. Ludwig, A. S. Sørensen, and P. Lodahl, Science 379, 389 (2023).
  * Ferioli  _et al._ [2021] G. Ferioli, A. Glicenstein, L. Henriet, I. Ferrier-Barbut, and A. Browaeys, Physical Review X 11, 021031 (2021), publisher: American Physical Society.
  * Rubies-Bigorda  _et al._ [2022] O. Rubies-Bigorda, V. Walther, T. L. Patti, and S. F. Yelin, Physical Review Research 4, 013110 (2022), publisher: American Physical Society.
  * Holzinger  _et al._ [2022] R. Holzinger, R. Gutiérrez-Jáuregui, T. Hönigl-Decrinis, G. Kirchmair, A. Asenjo-Garcia, and H. Ritsch, Physical Review Letters 129, 253601 (2022), publisher: American Physical Society.
  * Williamson  _et al._ [2020] L. Williamson, M. Borgh, and J. Ruostekoski, Physical Review Letters 125, 073602 (2020), publisher: American Physical Society.
  * Cidrim  _et al._ [2020] A. Cidrim, T. do Espirito Santo, J. Schachenmayer, R. Kaiser, and R. Bachelard, Physical Review Letters 125, 073601 (2020), publisher: American Physical Society.
  * Parmee and Ruostekoski [2020] C. D. Parmee and J. Ruostekoski, Communications Physics 3, 1 (2020), publisher: Nature Publishing Group.
  * Ferioli  _et al._ [2024] G. Ferioli, S. Pancaldi, A. Glicenstein, D. Clément, A. Browaeys, and I. Ferrier-Barbut, Physical Review Letters 132, 133601 (2024), publisher: American Physical Society.
  * Shah  _et al._ [2024] F. Shah, T. L. Patti, O. Rubies-Bigorda, and S. F. Yelin, Physical Review A 109, 012613 (2024), publisher: American Physical Society.
  * Ye and Zoller [2024] J. Ye and P. Zoller, Physical Review Letters 132, 190001 (2024).
  * Patti  _et al._ [2021] T. L. Patti, D. S. Wild, E. Shahmoon, M. D. Lukin, and S. F. Yelin, Physical Review Letters 126, 223602 (2021).
  * Masson and Asenjo-Garcia [2020] S. J. Masson and A. Asenjo-Garcia, Physical Review Research 2, 043213 (2020), publisher: American Physical Society.
  * Buckley-Bonanno  _et al._ [2022] S. Buckley-Bonanno, S. Ostermann, O. Rubies-Bigorda, T. L. Patti, and S. F. Yelin, Physical Review A 106, 053706 (2022), publisher: American Physical Society.
  * Walther  _et al._ [2006] H. Walther, B. T. H. Varcoe, B.-G. Englert, and T. Becker, Reports on Progress in Physics 69, 1325 (2006).
  * Sheremet  _et al._ [2023] A. S. Sheremet, M. I. Petrov, I. V. Iorsh, A. V. Poshakinskiy, and A. N. Poddubny, Reviews of Modern Physics 95, 015002 (2023), publisher: American Physical Society.
  * Bian  _et al._ [2021] K. Bian, W. Zheng, X. Zeng, X. Chen, R. Stöhr, A. Denisenko, S. Yang, J. Wrachtrup, and Y. Jiang, Nature Communications 12, 2457 (2021).
  * Bloom  _et al._ [2014] B. J. Bloom, T. L. Nicholson, J. R. Williams, S. L. Campbell, M. Bishof, X. Zhang, W. Zhang, S. L. Bromley, and J. Ye, Nature 506, 71 (2014).
  * Campbell and Hamilton [2017] W. C. Campbell and P. Hamilton, Journal of Physics B: Atomic, Molecular and Optical Physics 50, 064002 (2017).
  * Degen  _et al._ [2017] C. L. Degen, F. Reinhard, and P. Cappellaro, Reviews of Modern Physics 89, 035002 (2017).
  * Hsieh  _et al._ [2019] S. Hsieh, P. Bhattacharyya, C. Zu, T. Mittiga, T. J. Smart, F. Machado, B. Kobrin, T. O. Höhn, N. Z. Rui, M. Kamrani, S. Chatterjee, S. Choi, M. Zaletel, V. V. Struzhkin, J. E. Moore, V. I. Levitas, R. Jeanloz, and N. Y. Yao, Science 366, 1349 (2019), publisher: American Association for the Advancement of Science.
  * [40] See Supplemental Material for the derivation of the effective Hamiltonian, analytical constraints on $\Sigma$ and $\kappa$ and a discussion on exceptional points as well as the robustness to noise. .
  * Krämer  _et al._ [2016] S. Krämer, L. Ostermann, and H. Ritsch, Europhysics Letters 114, 14003 (2016), publisher: EDP Sciences, IOP Publishing and Società Italiana di Fisica.
  * Olmos  _et al._ [2014] B. Olmos, I. Lesanovsky, and J. P. Garrahan, Physical Review E 90, 042147 (2014).
  * Wiersig [2014] J. Wiersig, Physical Review Letters 112, 203901 (2014).
  * Heiss [2004] W. D. Heiss, Journal of Physics A: Mathematical and General 37, 2455 (2004).
  * Özdemir  _et al._ [2019] Ş. K. Özdemir, S. Rotter, F. Nori, and L. Yang, Nature Materials 18, 783 (2019).
  * Wiersig [2020a] J. Wiersig, Nature Communications 11, 2454 (2020a).
  * Wiersig [2020b] J. Wiersig, Photonics Research 8, 1457 (2020b).

Supplemental Material

##  I Realisation of effective Hamiltonian

Below we show how the effective Hamiltonian (3) can be obtained via adiabatic
elimination of the lattice Hamiltonian in the appropriate rotating wave
approximation. We model an array of $\ell$ atoms arranged in a lattice, as
well as two impurities with annihilation operators $a_{1},a_{2}$. We have a
space of size $N$, where the first $\ell$ entries correspond to the lattice
atoms, and the next two entries correspond to the impurities, i.e.,
$\ell=N-2$.

In real space, the Hamiltonian is split into

$\displaystyle H_{latt}$
$\displaystyle=\sum_{m}^{\ell}\left(\omega_{L}-i\frac{\gamma_{L}}{2}\right)\sigma_{m}^{{\dagger}}\sigma_{m}+\gamma_{L}\sum_{m\neq
n}^{\ell}\left(J_{mn}-i\frac{\Gamma_{mn}}{2}\right)\sigma^{{\dagger}}_{m}\sigma_{n}$
(S1)

We have also the interaction between the lattice and the impurities (or
defect)

$\displaystyle
H_{latt,d}=\sqrt{\gamma_{L}\gamma_{d}}\sum_{m}^{\ell}\left[\left(J_{dm}-i\frac{\Gamma_{dm}}{2}\right)a_{d}^{{\dagger}}\sigma_{m}+\left(J_{md}-i\frac{\Gamma_{md}}{2}\right)\sigma_{m}^{{\dagger}}a_{d}\right]$
(S2)

where $d=1,2$ indicate the impurity atoms and $\sigma_{z}^{(d)}$ indicates a
$\sigma_{z}$ matrix acting on site $d$. We have also the impurity interaction
that has the same form as the lattice atoms

$\displaystyle
H_{21}=\sqrt{\gamma_{2}\gamma_{1}}\left[\left(J_{21}-i\frac{\Gamma_{21}}{2}\right)a_{2}^{{\dagger}}a_{1}+\left(J_{12}-i\frac{\Gamma_{12}}{2}\right)a_{1}^{{\dagger}}a_{2}\right]$
(S3)

The bare impurity dynamics is governed by

$\displaystyle
H_{dd}=\sum_{d}\left(\omega_{d}-i\frac{\gamma_{d}}{2}\right)a_{d}^{{\dagger}}a_{d}$
(S4)

Due to resonant dipole-dipole interactions, we obtain collective coupling
$J_{ij}$ and decay $\Gamma_{ij}$ rates that are defined as

$\displaystyle\sqrt{\gamma_{i}\gamma_{j}}J_{ij}(\mathbf{r}_{i},\mathbf{r}_{j})$
$\displaystyle=-\frac{3\pi\sqrt{\gamma_{i}\gamma_{j}}}{\omega}\mathbf{d}^{{\dagger}}_{i}\cdot\mathrm{Re}\left[\mathbf{G}(\mathbf{r}_{ij},\omega)\right]\cdot\mathbf{d}_{j}$
(S5)
$\displaystyle\sqrt{\gamma_{i}\gamma_{j}}\Gamma_{ij}(\mathbf{r}_{i},\mathbf{r}_{j})$
$\displaystyle=-\frac{6\pi\sqrt{\gamma_{i}\gamma_{j}}}{\omega}\mathbf{d}^{{\dagger}}_{i}\cdot\mathrm{Im}\left[\mathbf{G}(\mathbf{r}_{ij},\omega)\right]\cdot\mathbf{d}_{j}$
(S6)

for the free space Green’s tensor $\mathbf{G}(\mathbf{r}_{ij},\omega)$ with
components

$G_{\alpha\beta}(\mathbf{r}_{ij},\omega)=\frac{e^{i\omega r}}{4\pi
r}\left[\left(1+\frac{i}{\omega
r}-\frac{1}{\omega^{2}r^{2}}\right)\delta_{\alpha\beta}-\left(1+\frac{3i}{\omega
r}-\frac{3}{\omega^{2}r^{2}}\right)\frac{r_{ij,\alpha}r_{ij,\beta}}{r^{2}}\right]-\frac{\delta(\mathbf{r}_{ij})}{3\omega^{2}}\delta_{\alpha\beta},$
(S7)

an atomic separation $\mathbf{r}_{ij}=\mathbf{r}_{i}-\mathbf{r}_{j}$,
$r\equiv|\mathbf{r}_{ij}|$, individual atomic decay rates
$\gamma_{i},\gamma_{j}$ and atomic dipole moments $\mathbf{d}_{i,j}$.

### Momentum space representation

Writing
$\sigma_{m}=\frac{1}{\sqrt{\ell}}\sum_{\mathbf{k}=1}^{\ell}e^{\frac{2\pi}{\ell}i\mathbf{k}\cdot\mathbf{r}_{m}}$,
moving to the rotating frame with frequency $\omega_{1}$ and requiring that we
are in the single-excitation subspace, we get that

$\displaystyle H_{latt}$
$\displaystyle=\gamma_{L}\sum_{\mathbf{k}}\left[\frac{\delta_{1L}}{\gamma_{L}}-\frac{i}{2}+J_{L}(\mathbf{k})-\frac{i}{2}\Gamma_{L}(\mathbf{k})\right]\sigma_{\mathbf{k}}^{{\dagger}}\sigma_{\mathbf{k}}$
(S8)

$\displaystyle
H_{latt,d}=\sqrt{\gamma_{L}\gamma_{d}}\sum_{\mathbf{k}}\left[\left(J_{d}(\mathbf{k})-\frac{i}{2}\Gamma_{d}(\mathbf{k})\right)a_{d}^{{\dagger}}\sigma_{\mathbf{k}}+\left(J^{*}_{d}(\mathbf{k})-\frac{i}{2}\Gamma^{*}_{d}(\mathbf{k})\right)\sigma_{\mathbf{k}}^{{\dagger}}d\right]$
(S9) $\displaystyle
H_{21}=\sqrt{\gamma_{1}\gamma_{2}}\left(J_{21}-i\frac{\Gamma_{21}}{2}\right)a_{2}^{{\dagger}}a_{1}+\left(J_{12}-i\frac{\Gamma_{12}}{2}\right)a_{1}^{{\dagger}}a_{2}$
(S10)

$\displaystyle H_{11}$
$\displaystyle=-i\frac{\gamma_{1}}{2}a_{1}^{{\dagger}}a_{1}$ (S11)
$\displaystyle H_{22}$
$\displaystyle=\left(\Delta-i\frac{\gamma_{2}}{2}\right)a_{2}^{{\dagger}}a_{2}$
(S12)

Where $\Delta=\omega_{2}-\omega_{1}$, $\delta=\omega_{L}-\omega_{1}$ and where
$\sigma_{\mathbf{k}}\to e^{-i\omega_{1}t}\sigma_{\mathbf{k}}$, $a_{1}\to
e^{-i\omega_{1}t}a_{1}$ and $a_{2}\to e^{-i\omega_{1}t}a_{2}$.

After adiabatic elimination of the lattice, we obtain

$\displaystyle\dot{c}_{1}$
$\displaystyle=-i\left(\gamma_{1}\Sigma_{1}-i\frac{\gamma_{1}}{2}\right)c_{1}-i\sqrt{\gamma_{1}\gamma_{2}}\kappa_{1}c_{2}$
(S13) $\displaystyle\dot{c}_{2}$
$\displaystyle=-i\left(\Delta+\gamma_{2}\Sigma_{2}-i\frac{\gamma_{2}}{2}\right)c_{2}-i\sqrt{\gamma_{1}\gamma_{2}}\kappa_{2}c_{1}$
(S14)

Where we define the self-energies

$\displaystyle\Sigma_{1}$
$\displaystyle\equiv\sum_{\mathbf{k}}\frac{(J_{1}(\mathbf{k})-\frac{i}{2}\Gamma_{1}(\mathbf{k}))(J^{*}_{1}(\mathbf{k})-\frac{i}{2}\Gamma^{*}_{1}(\mathbf{k}))}{\delta/\gamma_{L}+J_{L}(\mathbf{k})-\frac{i}{2}\Gamma_{L}(\mathbf{k})-i/2}$
(S15) $\displaystyle\Sigma_{2}$
$\displaystyle\equiv\sum_{\mathbf{k}}\frac{(J_{2}(\mathbf{k})-\frac{i}{2}\Gamma_{2}(\mathbf{k}))(J^{*}_{2}(\mathbf{k})-\frac{i}{2}\Gamma^{*}_{2}(\mathbf{k}))}{\delta/\gamma_{L}+J_{L}(\mathbf{k})-\frac{i}{2}\Gamma_{L}(\mathbf{k})-i/2}$
(S16)

and the effective coupling strengths

$\displaystyle\kappa_{1}$
$\displaystyle\equiv\left(J_{12}-i\frac{\Gamma_{12}}{2}+\sum_{\mathbf{k}}\frac{(J_{1}(\mathbf{k})-\frac{i}{2}\Gamma_{1}(\mathbf{k}))(J^{*}_{2}(\mathbf{k})-\frac{i}{2}\Gamma^{*}_{2}(\mathbf{k}))}{\delta/\gamma_{L}+J_{L}(\mathbf{k})-\frac{i}{2}\Gamma_{L}(\mathbf{k})-i/2}\right)$
(S17) $\displaystyle\kappa_{2}$
$\displaystyle\equiv\left(J_{21}-i\frac{\Gamma_{21}}{2}+\sum_{\mathbf{k}}\frac{(J_{2}(\mathbf{k})-\frac{i}{2}\Gamma_{2}(\mathbf{k}))(J^{*}_{1}(\mathbf{k})-\frac{i}{2}\Gamma^{*}_{1}(\mathbf{k}))}{\delta/\gamma_{L}+J_{L}(\mathbf{k})-\frac{i}{2}\Gamma_{L}(\mathbf{k})-i/2}\right)$
(S18)

Now calling $\ket{\psi}=\begin{pmatrix}c_{1}\\\ c_{2}\end{pmatrix}$ and noting
that $\kappa_{1}=\kappa_{2}\equiv\kappa$ and
$\Sigma_{1}=\Sigma_{2}\equiv\Sigma$, these equations can be written in terms
of an effective (non-Hermitian) Hamiltonian

$\displaystyle\mathcal{H}_{\mathrm{eff}}$
$\displaystyle=\left(\begin{array}[]{cc}-\frac{i\gamma_{1}}{2}+\gamma_{1}\Sigma&\sqrt{\gamma_{1}\gamma_{2}}\kappa\\\
\sqrt{\gamma_{1}\gamma_{2}}\kappa&-\frac{i\gamma_{2}}{2}+\Delta+\gamma_{2}\Sigma\\\
\end{array}\right)\equiv\left(\begin{array}[]{cc}\Omega_{1}-i\Gamma_{1}/2&\sqrt{\gamma_{1}\gamma_{2}}\kappa\\\
\sqrt{\gamma_{1}\gamma_{2}}\kappa&\Omega_{q}-i\Gamma_{2}/2\\\
\end{array}\right),$ (S23)

which is (3) in the main text.

### Constraints on $\Sigma$ and $\kappa$

The above derivation requires the computation of the self-energy $\Sigma$ and
the coupling rate $\kappa$ if the dynamics of the effective Hamiltonian (3)
are to be realised. Below we discuss the constraints and physical meaning of
these parameters.

The real part of $\Sigma$ is responsible for setting an energy scale, and so
does nothing whatsoever to the overall dynamics, as the only relevant quantity
affecting dynamics is the energy difference. To achieve the longest lifetime,
the requirement is $\mathrm{Im}\\{\Sigma\\}=1/2$, such that the decay terms
$\Gamma_{1}$ and $\Gamma_{2}$ are zero and thus the decay time diverges.
However due to the cosh term, at $\mathrm{Im}\\{\Sigma\\}=1/2$ we have a
positive overall decay, such that $\lim_{t\to\infty}|c_{1}(t)|^{2}\to\infty$,
i.e., the population diverges and is thus unphysical. In general, for us to
have physical parameters, we need to satisfy

$\displaystyle\mathrm{Im}\\{\Sigma\\}<\frac{1}{2}\,\,\,\mathrm{and}\,\,\,\bar{\gamma}\Gamma_{\mathrm{coop}}>2\mathrm{Im}\\{S\\},$
(S24)

where the second inequality comes from ensuring that
$\lim_{t\to\infty}|c_{1}(t)|^{2}<1$. Satisfying the second inequality puts
restrictions on $\kappa$. Assuming $\gamma_{1}=\gamma_{2}\equiv\gamma$, and
$\Delta=0$ we have $S=\sqrt{\gamma^{2}\kappa^{2}}$, and

$\displaystyle\mathrm{Im}(S)=\frac{\gamma\mathrm{Im}(\kappa)\mathrm{Re}(\kappa)}{|\mathrm{Re}(\kappa)|}.$
(S25)

Thus, the general condition under our assumptions is

$\displaystyle\frac{\gamma\mathrm{Im}(\kappa)\mathrm{Re}(\kappa)}{|\mathrm{Re}(\kappa)|}<\gamma\left(\frac{1}{2}-\mathrm{Im}(\Sigma)\right)$
(S26)

Assuming $\mathrm{Re}(\kappa)<0$ and $\mathrm{Im}(\kappa)<0$, we can further
simplify this to the condition

$\displaystyle\mathrm{Im}(\kappa)>\mathrm{Im}(\Sigma)-\frac{1}{2}.$ (S27)

##  II Exceptional points

In this section we compute the exceptional points of the system and find that
they lie far away from the optimal point for sensing in the presented
protocol. The eigenvectors of the effecive Hamiltonian (3) are

$\displaystyle
e_{\pm}=\begin{pmatrix}\frac{1}{\tilde{\kappa}}\left(\Omega_{12}-i\frac{\Gamma_{12}}{2}\pm
S\right)\\\ 1,\end{pmatrix}$ (S28)

with the eigenvalues given in the main text. The energies are degenerate when
the square root term
$S=\sqrt{-\left(i\Omega_{12}+\frac{\Gamma_{12}}{2}\right)^{2}+\tilde{\kappa}^{2}}=0$.
Equating real and imaginary parts, we obtain

$\displaystyle\gamma_{12}\Gamma_{\mathrm{coop}}$
$\displaystyle=\mathrm{Re}\\{\tilde{\kappa}\\}$ (S29)
$\displaystyle\frac{1}{2}\left(\Delta_{0}-\Delta\right)$
$\displaystyle=\mathrm{Im}\\{\tilde{\kappa}\\}$ (S30)

and this is the same point at which the eigenvalues also coalesce, i.e., it is
the exceptional point for the system. For any $\gamma_{1}\neq\gamma_{2}$, any
degeneracy in the eigenvalue spectrum is an exceptional point. However,
$\gamma_{1}=\gamma_{2}$ is the optimal point for the presented sensing
protocol, and the exceptional points lie far away (several orders of
magnitude) from this optimum. Therefore, we do not focus on the exceptional
points in this work.

##  III Optimal value of $R$

By computing $\sigma$ as a function of $R=\gamma_{1}/\gamma_{2}$ we found that
the optimal value of the gamma ratio is always $R=1$, i.e., that the two
impurities have the same decay rate, regardless of the value of $a$, and
additionally any value $R=r$ gives the same value of $\sigma$ as its
reciprocal $R=1/r$, mirroring the symmetry in the inset to Figure 2b.

##  IV Fisher information

The Fisher information for a binomially distributed quantity is given by
$\mathcal{I}(s)=\frac{n}{p(1-p)}$. The variance of a Bernoulli distribution
with $n$ trials is $np(1-p)$, and saturates the Cramér-Rao bound for a single
shot

$\displaystyle\sigma^{2}_{c_{\mathrm{signal}}}\geq\frac{1}{\mathcal{I}(s)}$
(S31) $\displaystyle np(1-p)\geq\frac{p(1-p)}{n}.$ (S32)

##  V Propagation of uncertainty for $\sigma_{\Delta_{\mathrm{signal}}}$

Each sampling event is a Bernoulli trial (the population is in the excited
state or it is not), where the probability of success is given by
$p=|c_{1}|^{2}$, corresponding to the probability of finding the detection
impurity in the excited state.

Given we know the uncertainty $\sigma^{2}_{p}=p(1-p)$, i.e., it has the well-
known variance for a Bernoulli distributed variable, we can propagate the
uncertainty through to $\Delta_{\mathrm{signal}}$, via

$\displaystyle\sigma_{p}^{2}$ $\displaystyle=\left(\frac{\partial
p}{\partial\Delta}\right)^{2}\sigma_{\Delta}^{2}$ (S33)
$\displaystyle\Rightarrow\sigma_{\Delta}$
$\displaystyle=\frac{\sigma_{p}}{\left|\frac{\partial
p}{\partial\Delta}\right|}.$ (S34)

Inserting the variance $\sigma^{2}_{p}$ yields

$\displaystyle\sigma_{\Delta}$
$\displaystyle=\frac{\sqrt{|c_{1}|^{2}(1-|c_{1}|^{2}})}{\left|\frac{\partial|c_{1}|^{2}}{\partial\Delta}\right|}$
(S35)

Here we note that the derivatives are evaluated at the added detuning
$\Delta_{\mathrm{add}}$.

##  VI Calculating the self-energies and effective coupling strengths for non-
periodic arrays

Bloch’s theorem can be applied for periodic lattices to obtain the lattice
dispersion used to determine the self-energies and coupling rates defined in
Eqs. (S16) and (S18). For impurities embedded in a non-periodic lattices, the
self-energy and coupling rates can be determined via an alternative method
based on the real space Hamiltonian.

In the single excitation manifold the system dynamics are governed by the
Schrödinger equation $i\partial_{t}\ket{\psi(t)}=H\ket{\psi(t)}$ with the non-
Hermitian Hamiltonian $H=H_{latt}+H_{latt,d}+H_{21}+H_{dd}$, with the
individual Hamiltonians defined in Eqs. (S1)–(S4). The atomic wave function
can be written as
$\ket{\psi(t)}=a(t)\ket{G,g_{1},g_{2}}+\sum_{m=1}^{l}b_{m}(t)e^{i\omega_{I}t}\ket{e_{i},g_{1},g_{2}}+c_{1}(t)e^{i\omega_{I}t}\ket{G,e_{1},g_{2}}+c_{2}(t)e^{i\omega_{I}t}\ket{G,g_{1},e_{2}}$,
where $\ket{G,g_{1},g_{2}}$ denotes the state with all atoms in the ground
state, $\ket{e_{i},g_{1},g_{2}}$ the state where only the $i$th lattice atom
is excited and $\ket{G,e_{1},g_{2}}$ ($\ket{G,g_{1},e_{2}}$) is the state
where only the impurity $1$ ($2$) is excited. In the frame rotating at the
impurity resonance frequency $\omega_{I}$, this results in a set of coupled
equations for the amplitudes $b_{m}(t)$, $c_{1}(t)$ and $c_{2}(t)$,

$\displaystyle\partial_{t}b_{m}(t)$
$\displaystyle=i\left(\delta_{LI}+\frac{i}{2}\gamma_{L}\right)b_{m}(t)-i\gamma_{L}\sum_{n\neq
m}^{l}\left(J_{mn}-\frac{i}{2}\Gamma_{mn}\right)b_{n}(t)$
$\displaystyle\quad-i\sqrt{\gamma_{L}\gamma_{1}}\left(J_{m1}-\frac{i}{2}\Gamma_{m1}\right)c_{1}(t)-i\sqrt{\gamma_{L}\gamma_{2}}\left(J_{m2}-\frac{i}{2}\Gamma_{m2}\right)c_{2}(t),$
(S36a) $\displaystyle\partial_{t}c_{1}(t)$
$\displaystyle=-\frac{\gamma_{1}}{2}c_{1}(t)-i\sqrt{\gamma_{L}\gamma_{1}}\sum_{m=1}^{l}\left(J_{m1}-\frac{i}{2}\Gamma_{m1}\right)b_{i}(t)-i\sqrt{\gamma_{1}\gamma_{2}}\left(J_{12}-i\frac{\Gamma_{12}}{2}\right)c_{2}(t)$
(S36b) $\displaystyle\partial_{t}c_{2}(t)$
$\displaystyle=-\frac{\gamma_{2}}{2}c_{2}(t)-i\sqrt{\gamma_{L}\gamma_{1}}\sum_{m=1}^{l}\left(J_{m2}-\frac{i}{2}\Gamma_{m2}\right)b_{i}(t)-i\sqrt{\gamma_{1}\gamma_{2}}\left(J_{21}-i\frac{\Gamma_{21}}{2}\right)c_{1}(t).$
(S36c)

We introduced the detuning $\delta_{LI}\coloneqq\omega_{I}-\omega_{L}$ between
the lattice and impurity atom transition frequencies. Note that the model does
not contain any classical driving terms so the derivatives of the excited
state populations don’t depend on the ground state population $a(t)$. This set
of equations can be written in matrix form

$i\begin{pmatrix}\dot{b}_{1}(t)\\\ \vdots\\\ \dot{b}_{l}(t)\\\
\dot{c_{1}}(t)\\\
\dot{c_{2}}(t)\end{pmatrix}=\begin{pmatrix}\leavevmode\nobreak\
&\leavevmode\nobreak\ &\leavevmode\nobreak\ &c_{1,1}&c_{1,2}\\\
\leavevmode\nobreak\ &\mathbf{H}_{L}&\leavevmode\nobreak\ &\vdots&\vdots\\\
\leavevmode\nobreak\ &\leavevmode\nobreak\ &\leavevmode\nobreak\
&c_{l,1}&c_{l,2}&\\\ c_{1,1}&\cdots&c_{1,l}&i\gamma_{1}/2&c_{1,2}\\\
c_{2,1}&\cdots&c_{2,l}&c_{2,1}&i\gamma_{2}/2\end{pmatrix}\cdot\begin{pmatrix}b_{1}(t)\\\
\vdots\\\ b_{l}(t)\\\ c_{1}(t)\\\ c_{2}(t)\end{pmatrix}$ (S37)

where the $l\times l$ matrix $\mathbf{H}_{L}$ represents the bare lattice
Hamiltonian matrix containing the terms $\propto(\delta_{LI}-i\gamma_{L}/2)$
in the diagonal and the coupling terms
$\propto\left(J_{mn}-i\Gamma_{mn}/2\right)$ in the off-diagonals [see first
line in Eq. (S36a)]. The complex numbers $c_{i,1}=c_{1,i}$ and
$c_{i,2}=c_{2,i}$ with $i\in[1,l]$ denote the coupling terms between the
lattice atoms and the impurity $\propto J_{m1}-i\Gamma_{m1}/2$ and $\propto
J_{m2}-i\Gamma_{m2}/2$, and $c_{1,2}$ = $c_{2,1}$ denotes the free space
coupling of the two impurities $1$ and $2$.

If $\gamma_{1,2}\ll\gamma_{L}$, the lattice dynamics can be adiabatically
eliminated. Defining the quantities $\mathbf{b}(t)\coloneqq(b_{1}(t)\ldots
b_{l}(t))^{T}$ and the lattice-impurity coupling vectors
$\mathbf{C}_{L1}\coloneqq(c_{1,1}\ldots c_{l,1})^{T}$,
$\mathbf{C}_{L2}\coloneqq(c_{1,2}\ldots c_{l,2})^{T}$ and setting
$\dot{b}_{i}(t)=0$ results in the steady state for the lattice atoms,

$\mathbf{b}_{ss}(t)=-\mathbf{H}_{L}^{-1}\cdot\left(\mathbf{C}_{L1}c_{1}(t)+\mathbf{C}_{L2}c_{2}(t)\right).$
(S38)

Plugging this back into Eq. (S37), we obtain the coupled set of equation
governing the impurities’ dynamics

$\displaystyle\dot{c_{1}}(t)$
$\displaystyle=-i\left[\frac{i}{2}\gamma_{1}-\mathbf{C}_{1L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L1}\right]c_{1}(t)-i\left[c_{1,2}-\mathbf{C}_{1L}^{T}\cdot\mathbf{H}^{-1}\mathbf{C}_{L2}\right]c_{2}(t),$
(S39) $\displaystyle\dot{c_{2}}(t)$
$\displaystyle=-i\left[\frac{i}{2}\gamma_{2}-\mathbf{C}_{2L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L2}\right]c_{2}(t)-i\left[c_{2,1}-\mathbf{C}_{2L}^{T}\cdot\mathbf{H}^{-1}\mathbf{C}_{L1}\right]c_{1}(t),$
(S40)

with $\mathbf{C}_{1L}^{T}\coloneqq(c_{1,1},\ldots,c_{1,l})$ and
$\mathbf{C}_{2L}^{T}\coloneqq(c_{2,1},\ldots,c_{2,l})$. To obtain the
effective Hamiltonian given in Eq. (S23) we have to bring these equations into
the form

$\displaystyle\dot{c_{1}}(t)=-i\left[\gamma_{1}\Sigma_{1}-i\frac{\gamma_{1}}{2}\right]c_{1}(t)-i\sqrt{\gamma_{1}\gamma_{2}}\kappa_{1}c_{2}(t),$
(S41)
$\displaystyle\dot{c_{2}}(t)=-i\left[\gamma_{2}\Sigma_{2}-i\frac{\gamma_{2}}{2}\right]c_{2}(t)-i\sqrt{\gamma_{1}\gamma_{2}}\kappa_{2}c_{1}(t).$
(S42)

This results in the following expressions for the self energies and coupling
strengths

$\displaystyle\Sigma_{1}\coloneqq-\mathbf{C}_{1L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L1}/\gamma_{1},$
(S43)
$\displaystyle\Sigma_{2}\coloneqq-\mathbf{C}_{2L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L2}/\gamma_{2},$
(S44)
$\displaystyle\kappa_{1}\coloneqq\left(c_{1,2}-\mathbf{C}_{2L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L1}\right)/\sqrt{\gamma_{1}\gamma_{2}},$
(S45)
$\displaystyle\kappa_{2}\coloneqq\left(c_{2,1}-\mathbf{C}_{1L}^{T}\cdot\mathbf{H}_{L}^{-1}\cdot\mathbf{C}_{L}\right)/\sqrt{\gamma_{1}\gamma_{2}}.$
(S46)

##  VII Robustness to noise

To illustrate the robustness of the introduced sensing protocol to external
noise, we analyze the role of positional disorder of the array atoms onto the
self-energy and coupling strength. Therefore, we randomly sample the lattice
positions for each emitter from a Gaussian distribution with a certain width
$\sigma$ centered around the unperturbed lattice points. We determine $\Sigma$
and $\kappa$ for 100 lattice realizations (for $R=\gamma_{1}/\gamma_{2}=1$)
and plot its mean value together with the standard deviation in Fig. S1. Note
that positional disorder breaks the lattice symmetry and Bloch’s theorem,
which was employed in the momentum space representation described above, is no
longer valid. Therefore, we perform the adiabatic elimination in real space as
outlined above.

The self-energy and the coupling strength are the two quantities at the core
of the introduced protocol.

Figure S1: Mean-value and standard deviation (indicated by blue and red shaded
regions) of the real- and imaginary-part of the self-energy $\Sigma$ (a)-(b)
and the coupling strength $\kappa$ (c)-(d) as a function of position disorder.
The insets show the lattice positions realized for a certain position
disorders.

The real part of the coupling constant $\kappa$ changes the time-scale of the
dynamics, i.e., it has a similar effect to changing the lattice spacing $a$.
As such, in Fig. S2a), we see a similar response to
$\mathrm{Re}(\kappa_{\mathrm{dis}})$ as changing $a$, including crossing over
resonance points. The imaginary part of $\kappa$ affects the decay lifetime in
a similar way to $\Gamma_{\mathrm{coop}}$. Increasing this parameter can
actually _improve_ the sensitivity, as seen in As discussed in SM I, the real
part of the self-energy $\Sigma$ simply sets an absolute energy scale and does
not affect the dynamics. As such, we do not see any change to $\sigma$ in
panel c) of Fig. S2. Finally, altering $\mathrm{Im}(\Sigma)$ directly affects
$\Gamma_{\mathrm{coop}}$ and thus can dramatically affect the sensitivity of
the protocol. In Fig. S1b), we see that relatively large amount of lattice
disorder is required before a significant change in $\mathrm{Im}(\Sigma)$ is
observed. In panel d) of Fig. S2, we see that the sensitivity remains
relatively low for small changes to $\Sigma_{\mathrm{dis}}$. However, directly
changing the imaginary part of $\Sigma$ is equivalent to changing the
cooperative enhancement factor $\Gamma_{\mathrm{coop}}$ directly. Since
improvements to this factor exponentially improve the sensitivity, disorder
that destroys this enhancement will similarly see a loss of this exponential
improvement. Hence we can see it is important that there is not too much
disorder that produces a large change in $\mathrm{Im}(\Sigma)$.

Figure S2: The protocol sensitivity $\sigma$ with the inclusion of lattice
disorder. a) $\sigma$ with added real part of the coupling constant
$\kappa_{\mathrm{dis}}$. b) Adding an imaginary $\kappa_{\mathrm{dis}}$. c)
Adding a real part $\Sigma_{\mathrm{dis}}$ to the self-energy $\Sigma$. c)
Adding an imaginary part $\Sigma_{\mathrm{dis}}$ to the self-energy $\Sigma$.


# Real-time Model Predictive Control with Zonotope-Based Neural Networks for
Bipedal Social Navigation

Abdulaziz Shamsah1,2, Krishanu Agarwal3, Shreyas Kousik1,∗, and Ye Zhao1,∗
1George W. Woodruff School of Mechanical Engineering, Georgia Institute of
Technology, Atlanta, GA, 30332-0405 USA<EMAIL_ADDRESS>Mechanical
Engineering Department, College of Engineering and Petroleum, Kuwait
University, PO Box 5969, Safat, 13060, Kuwait3School of Electrical and
Computer Engineering, Georgia Institute of Technology, Atlanta, GA 30308,
USA.∗Co-senior authorships.

###### Abstract

This study addresses the challenge of bipedal navigation in a dynamic human-
crowded environment, a research area that remains largely underexplored in the
field of legged navigation. We propose two cascaded zonotope-based neural
networks: a Pedestrian Prediction Network (PPN) for pedestrians’ future
trajectory prediction and an Ego-agent Social Network (ESN) for ego-agent
social path planning.Representing future paths as zonotopes allows for
efficient reachability-based planning and collision checking. The ESN is then
integrated with a Model Predictive Controller (ESN-MPC) for footstep planning
for our bipedal robot Digit designed by Agility Robotics. ESN-MPC solves for a
collision-free optimal trajectory by optimizing through the gradients of ESN.
ESN-MPC optimal trajectory is sent to the low-level controller for full-order
simulation of Digit. The overall proposed framework is validated with
extensive simulations on randomly generated initial settings with varying
human crowd densities.

##  I Introduction

Bipedal navigation in complex environments has garnered substantial attention
in the robotics community [1, 2, 3, 4, 5]. Social navigation is a particularly
challenging problem due to the inherent uncertainty of the environment,
unknown pedestrian dynamics, and implicit social behaviours [6]. Recently,
there has been an increasing focus on social navigation for mobile robots in
human environments [7, 8, 9, 10]. Nonetheless, the exploration of social
navigation for bipedal robots remains largely underexplored. This can be
attributed to the intricate hybrid, nonlinear, and high degrees-of-freedom
dynamics associated with bipedal locomotion.

In this study, we present an integrated framework for prediction and motion
planning for socially acceptable bipedal navigation in human-crowded
environments as shown in Fig. 1. We propose a navigation framework composed of
two cascaded neural networks: a Pedestrian Prediction Network (PPN) for
pedestrians’ future trajectory prediction and an Ego-agent Social Network
(ESN) for ego-agent social path planning. Our neural networks output reachable
sets for pedestrians and the ego-agent represented as zonotopes, a convex
symmetric polytope. Zonotopes offer efficient, yet robust reachability-based
planning, collision checking, and uncertainty parameterization [11, 12, 13,
14]. In this study, we use zonotopes to detect and avoid collisions by
checking for intersections between the zonotopes corresponding to the ego-
agent and pedestrians.

Our framework integrates ESN in a model predictive controller (MPC) as shown
in Fig. 2. The ESN-MPC optimizes over the output of the neural network, with
reachability and collision avoidance constraints. ESN-MPC incorporates a
reduced-order model (ROM) for the bipedal locomotion process and then sends
optimal commands, i.e., center of mass (CoM) velocity and heading change, to
the low-level controller on Digit for full-body joint trajectory design and
control.

Figure 1: Snapshot of the simulation environment with superimposed zonotopes
for the proposed reachability-based social navigation framework. The
environment is a $14$ m $\times$ $14$ m open space with $20$ pedestrians.

Figure 2: Block diagram of the proposed framework. The framework is composed
of two sub-networks: the Pedestrian Prediction Network (PPN) and the Ego-agent
Social Network (ESN) shown in green and cyan, respectively (Sec. V). Given an
environment with observed pedestrians and a goal location, PPN predicts the
future pedestrians’ reachable set. ESN-MPC optimizes through ESN to generate
collision-free trajectories for Digit (Sec. VI). The optimal trajectory is
then sent to the ALIP controller [15] to generate the desired foot placement
for reduced-order optimal trajectory tracking. An ankle-actuated-passivity-
based controller [16, 17] is implemented on Digit for full-body trajectory
tracking.

The main contributions of this study are as follows:

  * •

A zonotope-based prediction and planning framework for bipedal navigation in a
social environment.

  * •

Novel loss functions to shape zonotopes that represent the future social
trajectory of the ego-agent.

  * •

A framework for hierarchically integrating the neural networks with an MPC and
a low-level passivity controller for full-body joint control of Digit.

This article is outlined as follows. Section II is a literature review of
related work. Section III introduces the problem we are seeking to solve.
Then, the environment setup and zonotope preliminaries are in Section IV.
Section V presents the neural network architecture and loss functions.
Section. VI formulates the problem as an MPC. Implementation details and
results are in Section VII. Finally, Section VIII concludes this article.

##  II Related Work

Navigating an environment with humans in a socially compliant manner requires
a proactive approach to motion planning [7, 8, 9]. In [9], the authors use
opinion dynamics to proactively design motion plans for a mobile robot,
without the need for human prediction models. It relies only on the
observation of the approaching human position and orientation to form an
opinion that alters the neutral path and avoids collisions with pedestrians.
Gradient-based trajectory optimization is introduced in [8] to minimize the
difference between the humans’ future path prediction conditioned on the
robot’s plan and the nominal prediction. The studies of [7, 8] both assume
that a minimally-invasive robot trajectory, with minimal effect on surrounding
humans’ nominal trajectory, is socially acceptable. In contrast, our work aims
to learn the socially acceptable trajectory from human crowd datasets to
minimize any heuristic biases on what a socially acceptable trajectory is.

Our framework is inspired by the human trajectory prediction community [18,
19, 20, 21], where we aim to design a socially acceptable trajectory for the
ego-agent that mimics the path learned from human crowd datasets. The work in
[22] proposes an obstacle avoidance learning method that uses a Conditional
Variational Autoencoder (CVAE) framework to learn a temporary target
distribution to avoid pedestrians actively. However, during the learning
phase, the temporary targets are selected heuristically. In contrast, we aim
to learn such temporary waypoints from human crowd datasets to capture a
heuristic-free socially acceptable path. In [18], the authors develop a simple
yet, accurate CVAE architecture based on Multi-Layer Perceptrons (MLP)
networks to predict crowd trajectories conditioned on past observations and
intermediate endpoints. Our ESN follows a similar MLP-based CVAE architecture,
where the ego-agent path is conditioned on the final goal location,
surrounding pedestrians’ future trajectories, and immediate change in the ego-
agent state. Utilizing a non-complex network architecture is pivotal for
enabling real-time planning and prediction when integrated into gradient-based
motion planning for the ego-agent.

The authors in [11] present a Zonotope Alignment of Prediction and Planning
(ZAPP) that relies on zonotopes to enable continuous-time reasoning for
planning. They use trajectron++ [19] to predict obstacle trajectories as a
Gaussian distribution. They construct a zonotope over these distributions,
which leads to an overapproximation of the uncertainties. We propose learning
these distributions directly as zonotopes, bypassing the initial step of
predicting Gaussian distributions for pedestrian motion. This approach is
computationally efficient and facilitates real-time integration with an MPC.

##  III Problem formulation

###  III-A Robot Model

Figure 3: Illustration of the Linear Inverted Pendulum model for two
consecutive walking steps, with discrete states $\bm{p}_{q}$ and
$\bm{p}_{q+1}$ at the contact switching time. The shaded yellow regions
indicate the kinematics constraint on the control input $\bm{u}$ detailed in
Sec. VI-A.

Consider a bipedal ego-agent with discrete step-by-step dynamics
$\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$, where $\bm{x}_{q}$ and
$\bm{u}_{q}$ are the state and control input respectively at the contact
switching time of the $q^{\rm th}$ walking step. The robot’s state
$\bm{x}=(\bm{p},v^{\rm loc},\theta)$, where $\bm{p}=(x,y)$ is the 2-D location
in the global coordinate, $v^{\rm loc}$ is the local sagittal velocity, and
$\theta$ the heading. The control input is $\bm{u}_{q}=(u^{f}_{q}\quad
u^{\Delta\theta}_{q})$, where $u^{f}_{q}$ is the local sagittal foot position
relative to the CoM, and $u^{\Delta\theta}_{q}$ is the heading angle change as
shown in Fig. 3.

The reduced-order model (ROM) used to design the walking motion of Digit is
the Linear Inverted Pendulum (LIP) model [23]. For the LIP model, we assume
that each step has a fixed duration $T$111set to be equal to the timestep
between frames in the dataset ($0.4$ s) [2, 24]. Then we build our model on
the discrete local sagittal dynamics $(\Delta x^{\rm loc}_{q},v^{\rm
loc}_{q})$ 222the lateral dynamics are only considered in the ALIP model at
the low level since they are periodic with a constant desired lateral foot
placement (See Fig. 4), where $\Delta x^{\rm loc}=x^{\rm loc}_{q+1}-x^{\rm
loc}_{q}$ and $v^{\rm loc}_{q}$ is the sagittal velocity at the local
coordinate for the $q^{\rm th}$ walking step (see Fig. 3):

$\Delta x^{\rm loc}(u^{f}_{q})=\left(v^{\rm loc}_{q}\frac{\sinh(\omega
T)}{\omega}+(1-\cosh(\omega T))u^{f}_{q}\right)$ (1) $v^{\rm
loc}_{q+1}(u^{f}_{q})=\cosh(\omega T)v^{\rm loc}_{q}-\omega\sinh(\omega
T)u^{f}_{q}\cos(\theta_{q})$ (2)

where $\omega=\sqrt{g/H}$, where $g$ is the gravitational constant and $H$ is
the CoM height. Based on the local sagittal dynamics (1) and (2), we add
heading angle $\theta_{q}$ to control the LIP dynamics in 2-D Euclidean space.
The heading angle change is governed by
$\theta_{q+1}=\theta_{q}+u^{\Delta\theta}_{q}$ across walking steps. Therefore
the full LIP dynamics in 2-D Euclidean space become:

$\displaystyle x_{q+1}$ $\displaystyle=x_{q}+\Delta x^{\rm
loc}(u^{f}_{q})\cos(\theta_{q})$ (3a) $\displaystyle y_{q+1}$
$\displaystyle=y_{q}+\Delta x^{\rm loc}(u^{f}_{q})\sin(\theta_{q})$ (3b)
$\displaystyle v^{\rm loc}_{q+1}$ $\displaystyle=\cosh(\omega T)v^{\rm
loc}_{q}-\omega\sinh(\omega T)u^{f}_{q}\cos(\theta_{q})$ (3c)
$\displaystyle\theta_{q+1}$ $\displaystyle=\theta_{q}+u^{\Delta\theta}_{q}$
(3d)

For notation simplicity, hereafter, we refer to (3) as:

$\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ (4)

###  III-B Environment Setup and Problem Statement

The ego-agent is tasked to navigate to a known goal location $\mathcal{G}$ in
an open environment with $m\in\mathbb{N}$ observed pedestrians treated as
dynamic obstacles. The pedestrian state $\mathcal{T}^{p_{k}}_{[t_{p},t]}$ is
the 2-D trajectory of pedestrian $k$ observed over the discrete time interval
$[t_{p},t]$. The environment is partially observable as only the pedestrians
in a pre-specified sensory radius of the ego-agent are observed. The path the
ego-agent takes should ensure navigation safety, and promote social
acceptability.

######  Definition III.1 (Navigation safety).

Navigation safety is defined as maneuvering in human crowded environments
while avoiding collisions with pedestrians, i.e.,
$\|\bm{p}_{t}-\mathcal{T}^{p_{k}}_{t}\|>d,\;\forall t,k$, where $d$ represent
the minimum allowable distance between the ego-agent and the pedestrians.

######  Definition III.2 (Socially acceptable path for bipedal systems).

A path that a bipedal ego-agent takes in a human-crowded environment is deemed
socially acceptable if it has an Average Displacement Error (ADE) $<\epsilon$
333$\epsilon$ represents the allowable deviation from the socially acceptable
path. The Average Displacement Error denotes the average error between the
planned path and the ground-truth path. when compared to ground truth data in
the same environment.

Based on the aforementioned definitions and environment setup the problem we
aim to solve is as follows:

######  Problem III.1.

Given the discrete dynamics of the bipedal robot
$\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ and an environment state
$E=(\mathcal{T}^{p_{k}}_{[t_{p},t]},\mathcal{G})$, find a motion plan that
promotes social acceptability for the bipedal ego-agent in a partially
observable environment containing pedestrians while ensuring navigation
safety.

##  IV Preliminaries

To solve the social navigation problem defined above, we propose a learning
framework to learn socially acceptable reachable sets parameterized as
zonotopes (Sec. V-A).

Problem.III.1 is then reformulated as a step-by-step MPC problem with
navigation safety constraints and implemented in real time on our Digit
humanoid robot [25] (Sec. VI-D). This section begins by introducing the
learning and environment assumptions, and zonotope preliminaries.

####  IV-1 Environment Assumptions and Observations

In this work, we hypothesize that in a social setting, the information
accessible by the ego-agent that is used to determine its future path
$\mathcal{T}^{\rm ego}_{[t,t_{f}]}=\\{x^{\rm ego}_{q},y^{\rm
ego}_{q}\\}^{t_{f}}_{q=t}$444the subscripts $t_{p}$, $t$, and $t_{f}$
represent a discrete time indices denoting the past, current and future
trajectories, respectively, where $t_{p}<t<t_{f}$. are three fold: (i) its
final destination $\mathcal{G}=(x^{\rm dest},y^{\rm dest})$ (ego-agent
intent), (ii) the surrounding pedestrians’ past trajectory
$\mathcal{T}^{p_{k}}_{[t_{p},t]}=\\{x^{p_{k}}_{q},y^{p_{k}}_{q}\\}^{t}_{q=t_{p}}$
for the $k^{\rm th}$ pedestrian, and (iii) the ego-agent’s social experience,
i.e., its assumptions on how to navigate the environment in a socially-
acceptable manner. We treat the social experience as latent information that
is not readily available in human crowd datasets. Therefore we make the
following assumption.

######  Assumption IV.1.

Learning the future trajectory of an ego-agent $\mathcal{T}^{\rm
ego}_{[t,t_{f}]}$ based on its final goal $\mathcal{G}$ and surrounding
pedestrians’ past trajectories $\mathcal{T}^{p_{k}}_{[t_{p},t]}$, will learn
the ego-agent’s social experience.

Figure 4:  (a) shows the pedestrian prediction network, conditioned on the
pedestrian endpoints and the immediate change in the ego-agent’s state. (b)
shows the ego-agent social network conditioned on the pedestrians’ future
prediction, the immediate change in the ego-agent’s state, and the ego-agent’s
goal location. Dashed connections are used during training only.

####  IV-2 Zonotopes Preliminaries

A zonotope $\mathcal{Z}\in\mathbb{R}^{n}$ is a convex, symmetrical polytope
paramterized by a center $\bm{c}\in\mathbb{R}^{n}$ and a generator matrix
$G\in\mathbb{R}^{n\times n_{G}}$ (see Fig. 5).

$\mathcal{Z}={{\mathscr{Z}\\!}\\!\left(\bm{c},G\right)}=\\{\bm{c}+G\beta\;|\;\|\beta\|_{\infty}\leq
1\\}$ (5)

The Minkowski sum of
$\mathcal{Z}_{1}={{\mathscr{Z}\\!}\\!\left(\bm{c}_{1},G_{1}\right)}$ and
$\mathcal{Z}_{2}={{\mathscr{Z}\\!}\\!\left(\bm{c}_{2},G_{2}\right)}$ is
$\mathcal{Z}_{1}\oplus\mathcal{Z}_{2}=\mathscr{Z}{(\bm{c}_{1}+\bm{c}_{2},[G_{1}\;G_{2}])}$.
To Check collisions between two zonotopes, [26, Lemma 5.1] is used:

######  Proposition IV.2.

([26, Lemma 5.1]) $\mathcal{Z}_{1}\cap\mathcal{Z}_{1}=\emptyset$ iff
$\bm{c}_{1}\notin{{\mathscr{Z}\\!}\\!\left(\bm{c}_{2},[G_{1}\;G_{2}]\right)}$.

When $n=2$ zonotopes can be represented as polytopes using the half-space
representation $\mathcal{P}=\\{x\;|\;Ax\leq b\\}$, where
$x\in\mathcal{P}\iff\max(Ax-b)\leq 0$ and
$x\notin\mathcal{P}\iff\max(Ax-b)>0$. To convert a 2-D zonotope from the
center-generator representation to the half-space representation, we use the
following proposition:

######  Proposition IV.3.

([14, Theorem 2.1]) Let $C=\begin{bmatrix}-G[2,:]&G[1,:]]\end{bmatrix}$ and
$l_{G}[i]=\|G[:,i]\|_{2}$ the half-space representation of a 2-D zonotope:

$A[i,:]=\frac{1}{l_{G}[i]}\cdot\begin{bmatrix}C\\\
-C\end{bmatrix}\in\mathbb{R}^{2n_{G}\times 2}$ (6) $b=A\cdot
c+|{AG}|\;1_{m\times 1}\in\mathbb{R}^{2n_{G}}$ (7)

In this work, zonotopes are used to describe the social reachable set for the
ego-agent. We seek to learn a sequence of social zonotopes $\mathcal{Z}^{\rm
ego}_{q}$, each of which contains two consecutive waypoints of the ego-agent’s
future social trajectory $\mathcal{T}^{\rm ego}_{[t,t_{f}]}$.

######  Definition IV.1 (Social Zonotope $\mathcal{Z}^{\rm ego}_{q}$).

A social zonotope for the ego-agent’s $q^{\rm th}$ walking step is
$\mathcal{Z}^{\rm ego}_{q}=\mathscr{L}(\bm{c}_{q},G_{q})$, such that
$\mathcal{T}^{\rm
ego}_{[t,t_{f}]}\in\bigcup\limits_{q=t}^{t_{f}-1}\mathcal{Z}^{\rm ego}_{q}$.

##  V Social Zonotope Network

###  V-A Learning Architecture

We set up a conditional variational autoencoder (CVAE) architecture to learn
the ego-agent’s future trajectory conditioned on the final destination goal,
the immediate change in the ego-agent’s state, and the surrounding
pedestrians’ past trajectories. The proposed architecture incorporates Multi-
Layer Perceptrons (MLP) with ReLU non-linearity for all the sub-networks.

####  V-A1 Pedestrian Prediction Network (PPN)

The pedestrian prediction network (shown in Fig. 4(a)) is inspired by PECNet
[18], where the endpoint of the pedestrian trajectory
$\mathcal{T}^{p_{k}}_{t_{f}}$ is learned first, and then the future trajectory
is predicted. Our proposed network deviates from PECNet in three ways. First,
the pedestrian future trajectory is also conditioned on the immediate change
in the ego-agent’s state $\mathcal{T}^{\rm ego}_{t+1}$ (shown in red in Fig.
4(a)). This coupling of the pedestrian prediction and ego-agent planning
networks is intended to capture the effect of the robot’s control on the
future trajectories of the surrounding pedestrians [8, 6], and enable
bidirectional influence for the entire ego-agent-pedestrian team. Second, the
output of the network is the pedestrian’s future reachable set parameterized
as zonotopes $\mathcal{Z}^{p_{k}}_{[t,t_{f}]}$ rather than trajectories for
robust collision checking and uncertainty parameterization [11, 12, 13].
Third, we replace the social pooling module with a simple ego-agent sensory
radius threshold for computational efficiency.

The pedestrians’ past trajectories $\mathcal{T}^{p_{k}}_{[t_{p},t]}$ are
encoded in $E_{\rm ped}$ as seen by the purple arrow in Fig. 4(a), while the
incremental change in the ego-agent state representing the ego-agent control
is encoded in $E_{\rm nxt}$ as seen by the red arrow in Fig. 4(a). This allows
us to condition the prediction of the pedestrians’ trajectory on the ego-
agent’s control. The resultant latent features $E_{\rm
ped}(\mathcal{T}^{p_{k}}_{[t_{p},t]})$ and $E_{\rm nxt}(\mathcal{T}^{\rm
ego}_{t+1})$ are then concatenated and used as the condition features
$\mathbb{F}_{\rm cond}$. The pedestrian’s endpoint locations are encoded in
$E_{\rm end}$ as seen by the orange arrows in Fig. 4(a). The resultant latent
features $E_{\rm end}(\mathcal{T}^{p_{k}}_{t_{f}})$ are then concatenated with
$\mathbb{F}_{\rm cond}$ as global features $\mathbb{F}_{\rm global}$ and
encoded in the latent encoder $E_{\rm latent}$. We randomly sample features
from a normal distribution $\mathcal{N}(\bm{\mu},\bm{\sigma})$ generated by
the $E_{\rm latent}$ module, and concatenate them with $\mathbb{F}_{\rm
cond}$. This concatenated information is then passed into the latent decoder
$D_{\rm latent}$. Then $D_{\rm latent}$ outputs the predicted endpoint that is
passed again through $E_{\rm end}$. The output is concatenated again with
$\mathbb{F}_{\rm cond}$ and passed to $P_{\rm future}$ to output the predicted
zonotopes of the pedestrians $\mathcal{Z}^{p_{k}}_{[t,t_{f}]}$.

####  V-A2 Ego-agent Social Network (ESN)

ESN architecture is shown in Fig. 4(b). The surrounding pedestrians’ future
zonotope centers $\bm{c}^{p_{k}}_{[t,t_{f}]}$ are aggregated through summation
to take into account the collective effect of surrounding pedestrians while
keeping a fixed architecture555Other human trajectory learning modules include
a social module to take into account the surrounding pedestrians effect such
as social non-local pooling mask [18], max-pooling [20], and sorting [21].
[19]. The summed pedestrian features are then encoded in $E_{\rm future}$ as
seen by the green arrows in Fig. 4(b). The goal location for the ego-agent is
encoded in $E_{\rm goal}$, while the incremental change in the ego-agent state
is encoded in $E_{\rm nxt}$ as seen by the orange and red arrows respectively
in Fig. 4(b). The resultant latent features $E_{\rm
future}(\sum_{k=1}^{m}\bm{c}^{p_{k}}_{[t,t_{f}]})$, $E_{\rm
goal}(\mathcal{G})$ and $E_{\rm nxt}(\mathcal{T}^{\rm ego}_{t+1})$ are then
concatenated and used as the condition features $\mathbb{F}^{\rm ego}_{\rm
cond}$ for the CVAE. The ground truth of the ego-agent’s future trajectory
$\mathcal{T}^{\rm ego}_{[t,t_{f}]}$ is encoded in $E_{\rm traj}$ as shown by
the cyan arrows in Fig. 4(b). The resultant latent features $E_{\rm
traj}(\mathcal{T}^{\rm ego}_{[t,t_{f}]})$ are then concatenated with
$\mathbb{F}^{\rm ego}_{\rm cond}$ as global features $\mathbb{F}^{\rm
ego}_{\rm global}$ and encoded in the latent encoder $E_{\rm latent}$.
Similarly, we randomly sample features from a normal distribution
$\mathcal{N}(\bm{\mu},\bm{\sigma})$ generated by the $E_{\rm latent}$ module,
and concatenate them with $\mathbb{F}^{\rm ego}_{\rm cond}$. This concatenated
information is then passed into the latent decoder $D_{\rm latent}$, resulting
in our prediction of the ego-agent’s future reachable set $\mathcal{Z}^{\rm
ego}_{[t,t_{f}]}$.

######  Remark 1.

Including $E_{\rm nxt}$ in both neural networks facilitates seamless
integration with a step-by-step MPC, as the MPC’s decision variables
($\Delta\bm{p}^{\rm ego}$) will be used as inputs to $E_{\rm nxt}$ as detailed
in Sec. VI.

###  V-B Zonotope Shaping Loss Functions

Figure 5: Our zonotope shaping loss functions. The loss aims to learn
interconnected zonotopes that engulf the ground truth path.

The zonotope shaping loss functions are used for both PPN and ESN, where both
outputs are parameterized as zonotopes. The goal of these loss functions is
three folds: (i) penalize deviation of the centers of the zonotopes from the
ground truth future trajectory; (ii) generate intersecting zonotopes for
consecutive walking steps; and (iii) reduce the size of the zonotopes to avoid
unnecessary, excessively large zonotopes. Based on these goals, the following
is a list of loss functions to shape the zonotopes (Fig. 5):

  1. 1.

Average displacement error between the predicted centers and midpoint of the
ground truth trajectory $\mathcal{T}_{{\rm mid},i}$:

$\mathcal{L}_{ADE}=\frac{\sum_{i=1}^{t_{f}-1}\|\mathcal{T}_{{\rm
mid},i}-\bm{c}_{i}\|}{t_{f}-1}$

  2. 2.

Final displacement error between the last predicted center and the final
midpoint of the ground truth trajectory:

$\mathcal{L}_{FDE}=\|\mathcal{T}_{{\rm mid},t_{f}-1}-\bm{c}_{t_{f}-1}\|$

  3. 3.

The midpoint between the current center and previous center $\bm{c}^{p}_{{\rm
mid},i}$ is contained in the current zonotope:

$\mathcal{L}_{\rm prev}=\sum_{i=0}^{t_{f}-1}{\rm
ReLU}(A_{i}\cdot\bm{c}^{p}_{{\rm mid},i}-b_{i})$

  4. 4.

The midpoint between the current center and the next center $\bm{c}^{n}_{{\rm
mid},i}$ is contained in the current zonotope:

$\mathcal{L}_{\rm nxt}=\sum_{i=0}^{t_{f}-1}{\rm
ReLU}(A_{i}\cdot\bm{c}^{n}_{{\rm mid},i}-b_{i})$

  5. 5.

Regulating the size of the zonotope, by penalizing the norm of the generators
such the neural network does not produce excessively large zonotopes that
contain the ground truth trajectory:

$\mathcal{L}_{G}=\|l_{G}[1]-d_{1}\|+\|l_{G}[1:]-d_{2}\|$

where $d_{1}$ and $d_{2}$ are the desired lengths for the generators. We sum
the zonotope shaping losses listed above in a single term
$\mathcal{L}_{\mathcal{Z}}$. Similar to PECNet [18], we use Kullback–Leibler
divergence to train the output of the latent encoder, aiming to regulate the
divergence between the encoded distribution
$\mathcal{N}(\bm{\mu},\bm{\sigma})$and the standard normal distribution
$\mathcal{N}(0,\bm{I})$:

$\mathcal{L}_{KL}=D_{KL}\big{(}\mathcal{N}(\bm{\mu},\bm{\sigma})\|\mathcal{N}(0,\bm{I})\big{)}$

The network is trained end to end using the following loss function:
$\mathcal{L}=\mathcal{L}_{KL}+\mathcal{L}_{\mathcal{Z}}$.

##  VI Social MPC

To enable safe navigation in the human-crowded environment, we propose to
solve the following optimization problem:

$\displaystyle\min_{X,U}\quad$ $\displaystyle\sum_{q=0}^{N-1}J(\bm{x},\bm{u})$
(8a) s.t. $\displaystyle\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ (8b)
$\displaystyle\bm{x}_{0}=\bm{x}_{\rm
init},\;(\bm{x}_{q},\bm{u}_{q})\in\mathcal{XU}_{q}$ (8c)
$\displaystyle\bm{x}_{q+1}\in\mathcal{Z}^{\rm ego}_{q+1}(\Delta\bm{p}^{\rm
ego}_{q},E_{q})$ (8d) $\displaystyle\mathcal{Z}^{\rm
ego}_{q+1}(\Delta\bm{p}^{\rm
ego}_{q},E_{q})\bigcap\mathcal{Z}^{{p_{k_{q}}}}_{q+1}=\emptyset,\;\forall\;k_{q}$
(8e)

where the cost (8a) is designed to reach the goal and promote social
acceptability, subject to the ROM dynamics (8b) (Sec. III-A). Constraint (8d)
requires the ego-agent at the next $(q+1)^{\rm th}$ walking step to stay
within the reachable set, while constraint (8e) requires the ego-agent to
avoid collision with the pedestrians. Next, we introduce the kinematics,
reachability, and navigation constraints (Sec. VI-A-VI-B), and finally
reformulate the MPC in (8) with a detailed version for implementation (Sec.
VI-D).

###  VI-A Kinematics Constraints

To prevent the LIP dynamics from taking a step that is kinematically
infeasible by the Digit robot the following constraint is implemented

$\mathcal{XU}_{q}=\\{(\bm{x}_{q},\bm{u}_{q})\;|\;\bm{x}_{\rm
lb}\leq\bm{x}_{q}\leq\bm{x}_{\rm ub}\;\text{and}\;\bm{u}_{\rm
lb}\leq\bm{u}_{q}\leq\bm{u}_{\rm ub}\\}$ (9)

where $\bm{x}_{\rm lb}$ and $\bm{x}_{\rm ub}$ are the lower and upper bounds
of $\bm{x}_{q}$ respectively, and $\bm{u}_{\rm lb}$ and $\bm{u}_{\rm ub}$ are
the bounds for $\bm{u}_{q}$ (See yellow shaded region in Fig. 3). The detailed
parameters in our implementation are specified in Table II.

###  VI-B Reachability and Navigation Safety Constraints

To enforce navigation safety (i.e., collision avoidance), we require that
Digit remains in the social zonotope $\mathcal{Z}^{\rm ego}$ and outside of
the surrounding pedestrians reachable set $\hat{\mathcal{Z}}^{{p_{k}}}$.

####  VI-B1 Reachability constrains

For the robot’s CoM to remain inside the desired zonotope for the next walking
step $\mathcal{Z}^{\rm ego}_{q+1}$, we represent the zonotope using half-space
representation as shown in Prop. 6. The constraint is reformulated as such:

$\max(A^{\rm ego}\bm{p}^{\rm ego}-b^{\rm ego})\leq 0$ (10)

####  VI-B2 Navigation safety constraint

For pedestrian collision avoidance, we require that the reachable set of the
ego-agent does not intersect with that of the pedestrians for the
corresponding step. Therefore, we create a new zonotope for the ego-agent as
Minkowski sum of the ego-agent’s zonotope and the pedestrian’s zonotope
centered around the ego-agent $\mathcal{Z}^{\rm mink}=\mathscr{Z}(\bm{c}^{\rm
ego},[{G}^{{\rm ego}}\;G^{p_{k}}])$ to check for collision with the
pedesrians’ zonotope following Prop. IV.2. We then represent $\mathcal{Z}^{\rm
mink}$ using half-space representation and require that the pedestrian is
outside the combined set:

$\max(A^{\rm mink}\bm{p}_{k}-b^{\rm mink})>0$ (11)

###  VI-C Cost Function

The MPC cost function is designed to drive the ROM state to a goal location
$\mathcal{G}$. The terminal cost penalizes the distance between the current
ROM state and the global goal state $\mathcal{G}$.

$J_{N}(\bm{x}_{N})=\|\bm{x}_{N}-\bm{x}_{\mathcal{G}}\|^{2}_{W_{1}}+\|\theta_{N}-\theta_{\mathcal{G}}\|^{2}_{W_{2}}$
(12)

where $\bm{x}_{\mathcal{G}}=(\mathcal{G},v_{\rm terminal})$, and
$\theta_{\mathcal{G}}$ is the angle between the ego-agent’s current position
and the final goal location.

###  VI-D MPC Reformulation with Ego-agent Social Network

According to the aforementioned costs and constraints for implementation, we
reformulate our Ego-agent Social Network MPC (ESN-MPC) shown in (8) as
follows:

$\displaystyle\min_{X,U}\quad$
$\displaystyle\sum_{\mathfrak{q}=0}^{N-1}J_{N}(\bm{x}_{N})$ (13a) s.t.
$\displaystyle\bm{x}_{q+1}=\Phi(\bm{x}_{q},\bm{u}_{q})$ (13b)
$\displaystyle\bm{x}_{0}=\bm{x}_{\rm
init},\;(\bm{x}_{q},\bm{u}_{q})\in\mathcal{XU}_{q}$ (13c)
$\displaystyle\max(A^{\rm ego}_{q+1}\bm{p}^{\rm ego}_{q+1}-b^{\rm
ego}_{q+1})\leq 0$ (13d) $\displaystyle\max(A^{\rm
mink}_{q+1}\bm{p}_{k_{q+1}}-b^{\rm mink}_{q+1})>0,\;\forall\;k_{q}$ (13e)

##  VII Implementation and Results

###  VII-A Training

The social path planner module introduced in Sec. V was trained on the UCY
[27] and ETH [28] crowd datasets with the common leave-one-out approach,
reminiscent of prior studies [19, 18, 20].

The models were trained on a data set that excludes UNIV from the training
examples. We employ a historical trajectory observation
$\mathcal{T}^{p_{k}}_{[-8,0]}$ and a prediction horizon
$\hat{\mathcal{T}}^{\rm ego}_{[0,8]}$, each spanning a duration of $8$
timesteps ($3.2$ s) and only consider neighboring pedestrians that are within
a radius of $4$ m. The network architecture details are shown in Table I.

TABLE I: Network architecture parameters Pedestrian Prediction Network
---
$E_{\rm ped}$ | $16\rightarrow 32\rightarrow 16$
$E_{\rm end}$ | $2\rightarrow 8\rightarrow 16$
$E_{\rm nxt}$ | $2\rightarrow 32\rightarrow 16$
$P_{\rm future}$ | $50\rightarrow 32\rightarrow 16\rightarrow 32\rightarrow 70$
$E_{\rm latent}$ | $48\rightarrow 8\rightarrow 16\rightarrow 32$
$D_{\rm latent}$ | $48\rightarrow 32\rightarrow 16\rightarrow 32\rightarrow 2$
Ego-agent Social Network
$E_{\rm goal}$ | $2\rightarrow 8\rightarrow 16\rightarrow 2$
$E_{\rm future}$ | $16\rightarrow 64\rightarrow 32\rightarrow 16$
$E_{\rm nxt}$ | $2\rightarrow 64\rightarrow 32\rightarrow 2$
$E_{\rm traj}$ | $16\rightarrow 64\rightarrow 32\rightarrow 16$
$E_{\rm latent}$ | $36\rightarrow 8\rightarrow 50\rightarrow 16$
$D_{\rm latent}$ | $36\rightarrow 128\rightarrow 64\rightarrow 128\rightarrow 70$

###  VII-B Pedestrian Simulation

We use SGAN (Social Generative Adversarial Network), a state-of-the-art human
trajectory model, for simulating pedestrians [20]. SGAN is specifically
designed to grasp social interactions and dependencies among pedestrians. It
considers social context, including how people influence each other and move
in groups. This is important for creating realistic simulations of pedestrian
motion. Employing a different prediction model ensures a fair evaluation by
eliminating any inherent advantages of our proposed method[8].

In our simulation framework, SGAN incorporates both the historical
trajectories of pedestrians and the trajectory of the ego agent. This approach
enhances the realism of the simulation by accounting for the interaction
between the ego-agent and pedestrians within the environment.

###  VII-C Testing Environment Setup

The environment for all the following tests is an open space of $14\times 14$
m2 as shown in Fig. 1 and Fig. 8, with randomly generated pedestrians’ initial
trajectory. We test with $5$, $15$, and $30$ pedestrians in the environment.
The goal location is $\mathcal{G}=(10,10)$ m, and the ego-agent starting
position is uniformly sampled along the $y$-axis as such
$\bm{x}_{0}=(0,\mathcal{U}_{[0,13]},0)$ with $\theta_{0}=0$. The MPC is solved
with a planning horizon of $N=4$, and ESN-MPC parameters are included in
Table. II. Simulations and training are done using a 16-core Intel Xeon W-2245
CPU and an RTX-5000 GPU with 64 GB of memory.

TABLE II: ESN-MPC Parameters parameter | value | parameter | value
---|---|---|---
$u^{\Delta\theta}_{\rm ub}$ | $15^{\circ}$ | $u^{\Delta\theta}_{\rm lb}$ | $-15^{\circ}$
$u^{f}_{\rm ub}$ |  $0.4$ m | $u^{f}_{\rm lb}$ |  $-0.1$ m
$d_{1}$ | $0.1$ | $d_{2}$ | $0.005$
$v_{\rm terminal}$ |  $0$ m/s | $n_{G}$ | $4$
$W_{1}$ | $3$ | $W_{2}$ | $1$

###  VII-D Low-level Full-Body Control

At the low level we use the Angular momentum LIP planner introduced in [15],
and a Digit’s passivity controller [16] with ankle actuation which we have
previously shown to exhibit desirable ROM tracking results [17]. Here we set
the desired walking step time and the desired lateral step width to be fixed
at $0.4$ s and $0.4$ m, respectively.

###  VII-E Results and Discussion

Figure 6: Quantitative (a) and qualitative (b) results of ESN. (a) Shows the
displacement error between the prediction of ESN $\bm{c}^{\rm ego}$ (centers
of cyan zonotopes in (b)) and the ground truth data $\mathcal{T}^{\rm
ego}_{\rm mid}$ (red dots in (b)). (b) shows a snapshot of ESN output, where
the ego-agent’s predicted zonotopes (cyan) contain the ground truth ego-agent
data (red). ESN is conditioned on the goal position (yellow $\star$) and
surrounding pedestrian future trajectories (green). The data is collected
based on the UNIV dataset with $7831$ unique frames. The solid line in (a)
shows the average displacement error at each prediction horizon.

Figure 7: Full-order simulation results of Digit tracking the desired
trajectory from ESN-MPC. (a) shows Euclidean position tracking, (b) shows
heading tracking, and (c) shows the sagittal velocity tracking in local
coordinates.

Figure 8: Storyboard snapshots of ESN-MPC trajectory at different walking
steps. The ego-agent (cyan) successfully reaches the goal (yellow $\star$)
while avoiding pedestrians (green). The dashed circle is the sensory radius of
the ego-agent. Cayan dots represent the CoM of ROM, and black $\times$ is the
desired foot placement. Green dots are unobserved pedestrians.

Figure 9: ESN-MPC results: (a) velocity, (b) number of walking steps to reach
within $1$ m of the goal, (c) success rate, (d) frequency, and (e) minimum
distance to pedestrians. The data consists of 10 different trials with random
initial conditions and a fixed goal location (see Sec. VII-C). Each trail is
$100$ walking steps. The velocity data is collected before reaching the goal,
to avoid collecting a stop velocity. Success represents reaching within $1$ m
of the goal in $100$ steps. The dashed red line in (d) is the minimum required
frequency for full-order Digit simulation. The dashed red line (e) represents
$d$ in navigation safety (Definition. III.1).

In Fig. 6(a) we show that ESN produces an ADE$=0.229$ m over the prediction
horizon of $7$ timesteps666The prediction horizon timesteps is $7$ and not
$8$, since the displacement error is calculated based on the middle points
$\mathcal{T}^{\rm ego}_{\rm mid}$ of $\mathcal{T}^{\rm ego}_{[0,8]}$, and a
Final Displacement Error (FDE)$=0.52$ m. Fig. 6(b) shows a snapshot of the ESN
social zonotope output $\mathcal{Z}^{\rm ego}$ (cyan) compared to the ground
truth data $\mathcal{T}^{\rm ego}_{\rm mid}$ shown in red.

Fig. 7 shows the tracking performance of integrating ESN-MPC with the low-
level full-body controller [15, 16, 17]. We show the global Euclidean position
tracking Fig. 7(a), heading angle tracking in Fig. 7(b), and local sagittal
velocity tracking in Fig. 7(c). Fig. 1 and Fig. 8 show snapshots of the
resultant trajectory at different walking steps. A video of the simulations is
https://rb.gy/z9z3zj.

In Fig. 9(a), all three crowd densities produce relatively similar median
velocities. At lower crowd density the velocity is more consistent. As
expected, Fig. 9(b) shows that in less crowded areas, the ego-agent can reach
the goal in fewer steps. With $30$ pedestrians in the environment, it took
more steps on average to reach the goal while maintaining a relatively similar
velocity to the environments with fewer crowds (see Fig. 9(a)). This indicates
that our framework can predict the future trajectory of the surrounding
pedestrians, and is not required to come to a sudden stop. ESN-MPC produces a
consistent and predictable behavior for the ego-agent. Predictability of the
ego-agent behavior in a social context is desirable by pedestrians as it is
perceived to be less disruptive. With $5$ and $15$ pedestrians in the
environment our framework produced a $100\%$ success rate by reaching the goal
in $100$ walking steps, while it managed a $90\%$ success rate with $30$
pedestrians as shown in Fig. 9(c). Due to the larger number of constraints,
the time it takes to solve ESN-MPC decreases with increasing the number of
pedestrians (See Fig. 9(d)). However, even with $30$ pedestrians, the median
of the frequency is higher than the required minimum for Digit implementation
as indicated by the dashed red line in Fig. 9(d). Finally, ESN-MPC can
maintain a safe distance to the pedestrians in all three testing environments
as indicated in Fig. 9(e).

##  VIII Conclusion

This study introduced a novel framework for bipedal robot navigation in human
environments, addressing a significant gap in the field of locomotion
navigation. The proposed framework, which comprises the Pedestrian Prediction
Network (PPN) and the Ego-agent Social Network (ESN), leverages zonotopes for
efficient reachability-based planning and collision checking. Integrating ESN
with MPC for step planning for Digit showed promising results for safe
navigation in social environments. Future work will explore exploiting the
zonotope parameterization for modeling ROM and full-order model discrepancies,
introducing locomotion-specific losses to ESN training to facilitate safe
hardware experimentation, and quantifying social acceptability.

## References

  * [1] J.-K. Huang and J. W. Grizzle, “Efficient anytime clf reactive planning system for a bipedal robot on undulating terrain,” _IEEE Transactions on Robotics_ , 2023.
  * [2] K. S. Narkhede, A. M. Kulkarni, D. A. Thanki, and I. Poulakakis, “A sequential mpc approach to reactive planning for bipedal robots using safe corridors in highly cluttered environments,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 11 831–11 838, 2022.
  * [3] Y. Zhao, Y. Li, L. Sentis, U. Topcu, and J. Liu, “Reactive task and motion planning for robust whole-body dynamic locomotion in constrained environments,” _Int. J. Rob. Res._ , vol. 41, no. 8, pp. 812–847, 2022.
  * [4] S. Kulgod, W. Chen, J. Huang, Y. Zhao, and N. Atanasov, “Temporal logic guided locomotion planning and control in cluttered environments,” in _American Control Conference_. IEEE, 2020.
  * [5] J. Warnke, A. Shamsah, Y. Li, and Y. Zhao, “Towards safe locomotion navigation in partially observable environments with uneven terrain,” in _IEEE Conference on Decision and Control_ , 2020, pp. 958–965.
  * [6] C. Mavrogiannis, F. Baldini, A. Wang, D. Zhao, P. Trautman, A. Steinfeld, and J. Oh, “Core challenges of social robot navigation: A survey,” _ACM Transactions on Human-Robot Interaction_ , vol. 12, no. 3, pp. 1–39, 2023.
  * [7] M. Moder and J. Pauli, “Proactive robot movements in a crowd by predicting and considering the social influence,” in _2022 31st IEEE International Conference on Robot and Human Interactive Communication (RO-MAN)_. IEEE, 2022, pp. 644–651.
  * [8] S. Schaefer, K. Leung, B. Ivanovic, and M. Pavone, “Leveraging neural network gradients within trajectory optimization for proactive human-robot interactions,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2021, pp. 9673–9679.
  * [9] C. Cathcart, M. Santos, S. Park, and N. E. Leonard, “Proactive opinion-driven robot navigation around human movers,” in _2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2023, pp. 4052–4058.
  * [10] K. Majd, S. Yaghoubi, T. Yamaguchi, B. Hoxha, D. Prokhorov, and G. Fainekos, “Safe navigation in human occupied environments using sampling and control barrier functions,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2021, pp. 5794–5800.
  * [11] L. Paparusso, S. Kousik, E. Schmerling, F. Braghin, and M. Pavone, “Zapp! zonotope agreement of prediction and planning for continuous-time collision avoidance with discrete-time dynamics.”
  * [12] M. Selim, A. Alanwar, S. Kousik, G. Gao, M. Pavone, and K. H. Johansson, “Safe reinforcement learning using black-box reachability analysis,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 10 665–10 672, 2022.
  * [13] S. Kousik, P. Holmes, and R. Vasudevan, “Safe, aggressive quadrotor flight via reachability-based trajectory design,” in _Dynamic Systems and Control Conference_ , vol. 59162. American Society of Mechanical Engineers, 2019, p. V003T19A010.
  * [14] M. Althoff, “Reachability analysis and its application to the safety assessment of autonomous cars,” Ph.D. dissertation, Technische Universität München, 2010.
  * [15] Y. Gong and J. W. Grizzle, “Zero Dynamics, Pendulum Models, and Angular Momentum in Feedback Control of Bipedal Locomotion,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 144, no. 12, 10 2022, 121006.
  * [16] H. Sadeghian, C. Ott, G. Garofalo, and G. Cheng, “Passivity-based control of underactuated biped robots within hybrid zero dynamics approach,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2017, pp. 4096–4101.
  * [17] A. Shamsah, Z. Gu, J. Warnke, S. Hutchinson, and Y. Zhao, “Integrated task and motion planning for safe legged navigation in partially observable environments,” _IEEE Transactions on Robotics_ , pp. 1–22, 2023.
  * [18] K. Mangalam, H. Girase, S. Agarwal, K.-H. Lee, E. Adeli, J. Malik, and A. Gaidon, “It is not the journey but the destination: Endpoint conditioned trajectory prediction,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, 2020, Proceedings, Part II 16_. Springer, 2020, pp. 759–776.
  * [19] T. Salzmann, B. Ivanovic, P. Chakravarty, and M. Pavone, “Trajectron++: Dynamically-feasible trajectory forecasting with heterogeneous data,” in _Computer Vision–ECCV 2020: 16th European Conference, Glasgow, UK, August 23–28, 2020, Proceedings, Part XVIII 16_. Springer, 2020, pp. 683–700.
  * [20] A. Gupta, J. Johnson, L. Fei-Fei, S. Savarese, and A. Alahi, “Social gan: Socially acceptable trajectories with generative adversarial networks,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2018, pp. 2255–2264.
  * [21] A. Sadeghian, V. Kosaraju, A. Sadeghian, N. Hirose, H. Rezatofighi, and S. Savarese, “Sophie: An attentive gan for predicting paths compliant to social and physical constraints,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 1349–1358.
  * [22] Y. Hong, Z. Ding, Y. Yuan, W. Chi, and L. Sun, “Obstacle avoidance learning for robot motion planning in human-robot integration environments,” _IEEE Transactions on Cognitive and Developmental Systems_ , 2023.
  * [23] S. Kajita, F. Kanehiro, K. Kaneko, K. Yokoi, and H. Hirukawa, “The 3d linear inverted pendulum mode: A simple modeling for a biped walking pattern generation,” in _Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No. 01CH37180)_ , vol. 1. IEEE, 2001, pp. 239–246.
  * [24] S. Teng, Y. Gong, J. W. Grizzle, and M. Ghaffari, “Toward safety-aware informative motion planning for legged robots,” _arXiv preprint arXiv:2103.14252_ , 2021.
  * [25] A. Robotics, “Digit robot.” [Online]. Available: https://agilityrobotics.com/robots
  * [26] L. J. Guibas, A. T. Nguyen, and L. Zhang, “Zonotopes as bounding volumes.” in _SODA_ , vol. 3. Citeseer, 2003, pp. 803–812.
  * [27] A. Lerner, Y. Chrysanthou, and D. Lischinski, “Crowds by example,” in _Computer graphics forum_ , vol. 26, no. 3. Wiley Online Library, 2007, pp. 655–664.
  * [28] S. Pellegrini, A. Ess, K. Schindler, and L. van Gool, “You’ll never walk alone: Modeling social behavior for multi-target tracking,” in _2009 IEEE 12th International Conference on Computer Vision_ , 2009, pp. 261–268.