#  meSch: Multi-Agent Energy-Aware Scheduling for Task Persistence

Kaleb Ben Naveed1, An Dang1, Rahul Kumar1, and Dimitra Panagou1 ∗The authors
would like to acknowledge the support of the National Science Foundation (NSF)
under grant no. 2223845 and grant no. 1942907.1All authors are with the
Department of Robotics, University of Michigan, Ann Arbor, MI, 48109 USA.
{kbnaveed, andang, rahulhk<EMAIL_ADDRESS>
###### Abstract

This paper develops a scheduling protocol for a team of autonomous robots that
operate in long-term persistent tasks. The proposed framework, called meSch,
accounts for the robots’ limited battery capacity and the presence of a single
charging station, and achieves the following contributions: 1) First, it
guarantees exclusive use of the charging station by one robot at a time; the
approach is online, applicable to general nonlinear robot models, does not
require robots to be deployed at different times, and can handle robots with
different discharge rates. 2) Second, we consider the scenario when the
charging station is mobile and subject to uncertainty. This approach ensures
that the robots can rendezvous with the charging station while considering the
uncertainty in its position. Finally, we provide the evaluation of the
efficacy of meSch in simulation and experimental case studies.
[Code]aaaCodebase: https://github.com/kalebbennaveed/meSch.git[Video]bbbVideo:
https://youtu.be/8Hn8znYLPk0

##  I Introduction

Autonomous robots are extensively used for persistent missions over long time
horizons such as search and rescue operations [1], water body exploration [2],
ocean current characterization [3], and gas leakage inspection [4].

Trajectory planning for persistent missions requires not only optimizing high-
level objectives but also ensuring task persistence, realized in the sense
that robots must be able to return for recharging as needed. When there is
only one available charging station, the robots should coordinate their
schedules to ensure the charging station’s exclusive use. In this paper, we
consider the above challenges, including the case where the charging station
is mobile, and propose a solution framework meSch.

Recent work on task persistence has explored strategies for single-agent[5, 6,
7, 8, 9] as well as multi-agent scenarios[10, 11, 12, 13, 14, 15, 16, 17, 18,
19, 20, 21, 22, 23, 24, 25, 26]. In the realm of multi-agent task persistence,
existing methods explore scenarios with both static [10, 11, 12, 13, 14, 15,
16, 17, 18, 19, 20, 21] and mobile charging stations [21, 22, 23, 24, 25, 26,
27]. Most of the existing work on multi-agent task persistence with static
charging stations assumes either a dedicated charging station for each agent
[10, 11, 12, 21] or a single charging station that can cater to multiple
agents simultaneously [13, 14]. Additionally, some studies address the
scenarios where the number of charging stations available is fewer than the
number of robots [15, 16, 17]. They do this by either altering the nominal
paths to visit charging stations along the way [15] or strategically placing
the shared charging resources [16], or by imposing a constraint in the
optimization problem to ensure that only the number of the robots which are
equal to the number of the charging station comes back for recharging [17]. In
this paper, we explore a scenario where a team of robots exclusively shares a
single charging station.

The closest previous works to this setup are [19, 20]. [19] addressed the
challenge of sharing a single charging station among agents by deploying them
at different times. Conversely, [20] ensured exclusive use of the charging
station by manipulating the minimum allowed SoC (State-of-Charge) of each
robot’s battery using control barrier functions (CBF) [28]. While the CBF-
based approach developed in [20] is computationally efficient, it is
specifically tailored to the single-integrator model. In this paper, we
propose an online scheduling method that ensures the exclusive use of the
single charging station. The proposed method does not require robots to be
deployed at different times or with different SoC capacities and applies to
nonlinear robot models, enhancing its generalizability. Additionally, unlike
[19, 20], we also address the scenario when the charging station is mobile.

Much of the literature addressing mobile charging stations considers that one
of the robots in the team serves as a mobile charging station. Some approaches
pre-plan the robots’ paths for the entire mission to determine appropriate
rendezvous points where robots will meet for recharging during the mission
[22, 23]. Others assume continuous communication between the robots to
determine the rendezvous points online whenever one of the robots needs to
recharge [24]. The other direction of work deploys separate mobile charging
robot(s), and plans their paths to recharge the robots involved in the mission
along their nominal paths [25, 26, 27].[27] also consider the presence of
obstacles while planning paths for the mobile charging station.

In this paper, we consider a scenario when one of the robots involved in the
persistent mission also serves as a charging station. Furthermore, we adopt a
stochastic model for the charging station dynamics to tackle uncertainties
stemming from erroneous state estimation or external disturbances. With this
consideration in mind, we devise a method to robustly estimate the rendezvous
points online, incorporating the uncertainty in the charging station’s
position.

The paper is organized as follows: Section II formulates the problem
statement, Section III presents the proposed solution, and Section IV discuss
results.

##  II Problem Formulation

###  II-A Notation

Let $\mathbb{Z}_{0}=\\{0,1,2,...\\}$ and $\mathbb{Z}_{+}=\\{1,2,3,...\\}$. Let
$\mathbb{R}$, $\mathbb{R}_{\geq 0}$, $\mathbb{R}_{>0}$ be the set of reals,
non-negative reals, and positive reals respectively. Let
$\mathcal{S}^{n}_{++}$ denote set of symmetric positive-definite matrices in
$\mathbb{R}^{n\times n}$. Let $\mathcal{N}(\mu,\Sigma)$ denote a normal
distribution with mean $\mu$ and covariance $\Sigma\in\mathcal{S}^{n}_{++}$.
The $Q\in\mathbb{S}_{++}^{n}$ norm of a vector $x\in\mathbb{R}^{n}$ is denoted
$\left\|x\right\|_{Q}=\sqrt{x^{T}Qx}.$

###  II-B System Description

Consider a multi-agent system, in which each robotic system
$i\in\mathcal{R}=\\{0,1,\cdots,N-1\\}$ comprises the robot and battery
discharge dynamics:

$\dot{\chi}^{i}=\begin{bmatrix}\dot{x}^{i}\\\
\dot{e}^{i}\end{bmatrix}=f^{i}(\chi^{i},u^{i})\\\
=\begin{bmatrix}f_{r}^{i}(x^{i},u^{i})\\\ f_{e}^{i}(e^{i})\end{bmatrix},$ (1)

where $N=|\mathcal{R}|$ is the cardinality of the set $\mathcal{R}$,
$\chi^{i}=\begin{bmatrix}{x^{i}}^{T},&e^{i}\end{bmatrix}^{T}\in\mathcal{Z}^{i}_{r}\subset\mathbb{R}^{n+1}$
is the $i^{th}$ robotic system state consisting of the robot state
$x^{i}\in\mathcal{X}^{i}_{r}\subset\mathbb{R}^{n}$ and its State-of-Charge
(SoC) $e^{i}\in\mathbb{R}_{\geq 0}$.
$u^{i}\in\mathcal{U}^{i}_{r}\subset\mathbb{R}^{m}$ is the control input,
$f^{i}:\mathcal{Z}^{i}_{r}\times\mathcal{U}^{i}_{r}\rightarrow\mathbb{R}^{n+1}$
defines the continuous-time robotic system dynamics,
$f_{r}^{i}:\mathcal{X}^{i}_{r}\times\mathcal{U}^{i}_{r}\rightarrow\mathbb{R}^{n}$
define robot dynamics and $f_{e}^{i}:\mathbb{R}_{\geq 0}\to\mathbb{R}$ define
worst-case battery discharge dynamics. We also consider the continuous-time
dynamics of the mobile charging station:

$\displaystyle\dot{x}^{c}$ $\displaystyle=f_{c}(x^{c},u^{c})+w(t),$
$\displaystyle w(t)\sim\mathcal{N}(0,W(t)),$ (2a) $\displaystyle y^{c}$
$\displaystyle=z(x^{c})+v(t),$ $\displaystyle v(t)\sim\mathcal{N}(0,V(t)),$
(2b)

where $x^{c}\in\mathcal{X}_{c}\subset\mathbb{R}^{c}$ is the charging station
state, $u^{c}\in\mathcal{U}_{c}\subset\mathbb{R}^{s}$ is the charging station
control input,
$f_{c}:\mathcal{X}_{c}\times\mathcal{U}_{c}\rightarrow\mathbb{R}^{c}$ defines
the continuous-time system dynamics for the mobile charging, $w(t)$ is the
time-varying process noise with zero mean and known variance
$W(t)\in\mathbb{R}_{\geq 0}$, $y\in\mathbb{R}^{c}$ is the measurement,
$z:\mathbb{R}^{c}\to\mathbb{R}^{c}$ is the observation model, and $v(t)$ is
the time-varying measurement noise with zero mean, and known covariance
$V(t)$.

Figure 1: meSch: Multi-Agent Energy-Aware Scheduling

###  II-C Problem statement

Consider a team of $N+1$ robots performing a persistent mission. Among them,
$N$ robots, modeled by (1), are the rechargeable robots, require recharging,
while $1$ robot, modeled by (2), serves as the mobile charging robot that
doesn’t require recharging.cccThis could represent a ground vehicle with a
battery lasting a few hours. This assumption has been also made in previous
works [25, 27]. We assume continuous communication and the existence of a
high-level trajectory planner, which plans the nominal trajectories for all
$N+1$ robots according to the high-level mission objective. We pursue the
following two objectives for the rechargeable robots:

1) adhere to the nominal trajectories and deviate from them only when
returning for recharging.

2) ensure mutually exclusive use of the mobile charging robot, which strictly
follows its nominal trajectory.

Before formulating an optimization problem, we define $\mathcal{T}^{i}$ as the
set of times $i^{th}$ robot returns to the charging station:

$\displaystyle\mathcal{T}^{i}=\\{t_{s,0}^{i},t_{s,1}^{i},\cdots,t_{s,m}^{i},\cdots\\},\forall
i\in\mathcal{R},\forall m\in\mathbb{Z}_{0}$ (3)

where $t_{s,m}^{i}$ denotes the $m^{th}$ time $i^{th}$ rechargeable robot
returns to the charging station. Then, let
$\mathcal{T}=\cup_{i\in\mathcal{R}}\mathcal{T}^{i}$ be the union of return
times for all robots. Subsequently, the trajectory optimization problem is
formulated as follows:

$\displaystyle\min_{\chi^{i}(t),u^{i}(t),\mathcal{T}}\ $
$\displaystyle\sum_{i=0}^{N-1}\int_{t_{0}}^{\infty}\Bigl{[}\left\|x^{i}(\tau)-x^{i,nom}_{0}(\tau)\right\|_{\mathbf{Q}}^{2}+\left\|u^{i}(\tau)\right\|_{\mathbf{R}}^{2}\Bigr{]}d\tau$
$\displaystyle\quad\quad\quad+|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|\quad\forall
t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$ (4a) s.t.
$\displaystyle\chi^{i}(t_{0})=\chi_{0}^{i}\quad\quad\forall i\in\mathcal{R}$
(4b) $\displaystyle\dot{\chi}^{i}=f(\chi^{i},u^{i})\quad\quad\forall
i\in\mathcal{R}$ (4c) $\displaystyle\dot{x}^{c}=f_{c}(x^{c},u^{c})+w(t)$ (4d)
$\displaystyle e^{i}(t)\geq{e^{i}_{min}}\quad\quad\forall i\in\mathcal{R}$
(4e)
$\displaystyle|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|>T_{ch}+T_{\delta}\quad\forall
t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$ (4f) $\displaystyle
x^{i}(t_{s,m}^{i})=x^{c}(t_{s,m}^{i})\quad\forall i\in\mathcal{R},\forall
m\in\mathbb{Z}_{0}$ (4g)

where $x^{i,nom}_{0}=x^{i}([t_{0},\infty];t_{0},x_{0})$ refers to the nominal
trajectory of the $i^{th}$ rechargeable robot generated by the high-level
trajectory planner at the initial time $t_{0}$ and state $x^{i}_{0}$. $T_{ch}$
is the charging time, and $T_{\delta}$ is the buffer time. The cost functional
(4) has three components. The first and second terms minimize the state and
control costs, respectively. The third term aims to minimize the time interval
between the start of charging for any two rechargeable robots. This is to
encourage the rechargeable robots to follow the nominal trajectory as much as
possible before returning for recharging. The constraint (4e) specifies the
minimum battery SoC constraints for all rechargeable robots. (4f) sets the
lower bound for the time interval between the return of any two rechargeable
robots. This constraint ensures that only one rechargeable robot is present at
the mobile charging robot at a given time. Finally, (4g) requires the
rechargeable robot to dock at the mobile charging robot upon return for
recharging.

##  III Multi-agent energy-aware scheduling (meSch)

###  III-A Method Overview

Building on our prior work [9, 29], we introduce meSch as a 4-step approximate
solution to the problem (4). Within the persistent planning framework shown in
Figure 1, meSch serves as a low-level module to the high-level path planner
ensuring task persistence. meSch ensures that the rechargeable robots follow
the nominal trajectories, and only deviate from them either if the minimum SoC
constraint (4e) or the minimum gap constraint (4f) is predicted to be violated
in the future. Moreover, meSch ensures that as a rechargeable robot returns
for recharging, it can rendezvous with the mobile charging robot despite
stochastic dynamics. The proposed solution is developed under a few
assumptions:

######  Assumption 1.

At each iteration of meSch, the nominal trajectories of the rechargeable
robots and the mobile charging robot are known for at least $T_{N}$ and
$T_{R}$ seconds in the future respectively. This can be ensured by replanning
nominal trajectories at an adequate frequency. The method to determine $T_{N}$
and $T_{R}$ is described later.

The meSch module runs every $T_{E}<T_{N}$ seconds. That is, at each time
$t_{j}=jT_{E},j\in\mathbb{Z}_{0}$,

(A) Estimate the state of the mobile charging robot at $t_{j}+T_{R}$ in the
future using the Extended Kalman Filter (EKF). Then, position the rendezvous
point $d$ meters above the estimated state. This rendezvous point corresponds
to the location where the rechargeable robot will return.

(B) For each rechargeable robot, construct a candidate trajectory spanning
$T_{C}$ seconds into the future to end at the rendezvous point. It initially
follows the nominal trajectory for $T_{N}$ seconds, then attempts to reach the
rendezvous point within $T_{B}$ seconds. These time horizons are user-defined
and must satisfy $T_{C}=T_{N}+T_{B}$.

(C) Calculate the reserve energy needed for the rechargeable robot to perform
a worst-case landing maneuver within $T_{L}$ seconds, i.e. to go from the
rendezvous point to any point within the 95% confidence interval of the
estimated mobile charging robot state at $t_{j}+T_{R}$. $T_{L}$ is tuned such
that $T_{L}=T_{R}-T_{C}$.

(D Commit the candidate trajectories if they satisfy the minimum SoC
constraint (4e) and the minimum gap constraint (4f). The output of this final
block is the committed trajectories for each robot $i\in\mathcal{R}$, which
are tracked by the low-level controller.

Before discussing each block of meSch in detail, we establish the notation for
trajectories. Let $x^{i}([t_{j},t_{j}+T_{N}];t_{j},x^{i}_{j})$ be the nominal
trajectory for the $i^{th}$ rechargeable robot available at time $t_{j}$
starting at state $x^{i}_{j}$ and defined over a time horizon $T_{N}$ seconds.
For compactness, we refer to $x^{i}([t_{j},t_{j}+T_{N}];t_{j},x^{i}_{j})$ as
$x^{i,nom}_{j}$. We use similar notation to describe other trajectories later
on. Furthermore, Table I provide an overview of the notation we use throughout
the methodology.

Symbol |  Definition
---|---
Indices
$i$ |  Rechargeable robot index
$j$ |  meSch iteration index
$k$ |  Rechargeable robot index in sorted list
Time Horizons
$T_{ch}$ |  Recharging duration
$T_{\delta}$ |  Buffer time
$T_{N}$ |  Nominal trajectory
$T_{B}$ |  back-to-base trajectory
$T_{C}$ |  Candidate trajectory ($T_{C}=T_{N}+T_{B}$)
$T_{L}$ |  Worst-case landing time
$T_{R}$ |  $T_{R}=T_{C}+T_{L}$
$T_{E}$ |  Time interval between $j$ and $j+1$ iteration
$T^{k}_{F,j}$ |  Remaining battery time of the $k^{th}$ robot in the sorted list at time $t_{j}$
Time Points
$t_{j}$ |  Start time of iteration $j$
$t_{j,N}$ |  $t_{j}+T_{N}$
$t_{j,C}$ |  $t_{j}+T_{C}$
$t^{i}_{j,R}$ |  $t_{j}+T_{C}+T^{i}_{R,j}$
$t_{s,m}^{i}$ |  $m^{th}$ time $i^{th}$ robot returns for recharging
Table I: Time and Index Notation at a glance

###  III-B Estimating Rendezvous point

At the $j^{th}$ iteration of meSch, we estimate a rendezvous point to which
the rechargeable robot would return if it were to come back. The rendezvous
point corresponds to the position $d$ meters above the estimated state of the
mobile charging robot at time $t_{j,R}$, i.e., $\hat{x}^{c}(t_{j,R})$.

Given the current state estimate $\hat{x}^{c}(t_{j})$ and its covariance
$\Sigma^{c}(t_{j})$ from the EKF, we forward propagate the current estimate
using the EKF predict equations[30] to estimate the mobile charging robot
state at $t_{j,R}$ in the future, i.e. $\hat{x}^{c}(t_{j,R})$ and
$\Sigma^{c}(t_{j,R})$. Alongside the estimated state $\hat{x}^{c}(t_{j,R})$,
we also determine the worst-case state. During this calculation, we make the
following assumption:

######  Assumption 2.

The true state belongs to the estimated state’s 95% confidence error ellipse.
Based on this, the worst-case state estimate $\hat{x}^{c}_{w}(t_{j,R})$ is
computed as follows:

$\displaystyle\hat{x}^{c}_{w}(t_{j,R})=\hat{x}^{c}(t_{j,R})+1.96\sqrt{\Psi(\Sigma^{c}(t_{j,R}))}$
(5)

where $1.96$ is the z-score for the $95\%$ confidence interval,
$\Psi:\mathbb{R}^{c}\times\mathbb{R}^{c}\to\mathbb{R}^{c}$ returns a vector
comprising the diagonal elements of $\Sigma^{c}(t_{j,C})$, and $\sqrt{.}$
operation is performed element-wise. Finally, the rendezvous point is set as:

$\displaystyle x^{c}_{rp}=\hat{x}^{c}(t_{j,R})+[0_{1\times
2},d,0_{1\times(c-3)}]^{T}$ (6)

where $d\in\mathbb{R}_{>0}$ is added to the z-dim of the state and $c$ is the
mobile charging robot state dimension as given in (2).

###  III-C Construction of Candidate Trajectories

Figure 2: This figure illustrates the generation of candidate trajectories at
time $t_{j}$. All the candidate trajectories terminate at the rendezvous point
$x^{c}_{rp}$ at time $t_{j,C}$.

Inspired by [29], given nominal trajectories $x^{i,nom}_{j}$ $\forall
i\in\mathcal{R}$, we construct a candidate trajectory for each rechargeable
robot that tracks a portion of the nominal trajectory and then attempt to
reach the rendezvous point $x^{c}_{rp}$ within $T_{B}$ seconds. For the
$i^{th}$ rechargeable robot, the candidate trajectory is constructed by
concatenating the nominal trajectory with a back-to-base (b2b) trajectory.

Let the $i^{th}$ rechargeable robot state at time $t_{j}$ be
$x^{i}_{j}\in\mathcal{X}$ and the system state at $t_{j}$ be
$\mathcal{X}^{i}_{j}\in\mathcal{Z}^{i}_{r}$. For $i^{th}$ rechargeable robot,
we construct a back-to-base (b2b) trajectory $x^{i,b2b}_{j}$ defined over
interval [$t_{j,N},t_{j,C}$] by solving the following optimal control problem:

$\displaystyle\min_{x^{i}(t),u^{i}(t)}\ $
$\displaystyle\int_{t_{j,N}}^{t_{j,C}}\left\|x^{i}(t)-x^{c}_{rp}\right\|_{\mathbf{Q}}^{2}+\left\|u^{i}(t)\right\|_{\mathbf{R}}^{2}dt$
(7a) s.t. $\displaystyle x^{i}(t_{j,N})=x_{j}^{i,nom}(t_{j,N})$ (7b)
$\displaystyle\dot{x}=f_{r}^{i}(x^{i},u^{i})$ (7c) $\displaystyle
x^{i}(t_{j,C})=x^{c}_{rp}$ (7d)

where $\mathbf{Q}\in\mathbb{S}_{++}^{n}$ weights state cost,
$\mathbf{R}\in\mathbb{S}_{++}^{m}$ weights control cost. Once b2b trajectory
$x_{j}^{i,b2b}$ is generated, we numerically construct the system candidate
trajectory

$\displaystyle\chi^{i,can}_{j}$
$\displaystyle=\begin{cases}x^{i,can}_{j}(t),&t\in[t_{j},t_{j,C})\\\
e^{i,can}_{j}(t),&t\in[t_{j},t_{j,C})\end{cases}$ (8)

over a time interval [$t_{j},t_{j,C}$) by solving the initial value problem
for each rechargeable robot system, i.e.

$\displaystyle\dot{\chi}^{i}$ $\displaystyle=f(\chi^{i},u^{i}(t)),$ (9a)
$\displaystyle\chi^{i}(t_{j})$ $\displaystyle=\chi^{i}_{j}$ (9b)
$\displaystyle u^{i}(t)$
$\displaystyle=\begin{cases}\pi^{i}_{r}(\chi^{i},x^{i,nom}_{j}(t)),&t\in[t_{j},t_{j,N})\\\
\pi^{i}_{r}(\chi,x_{j}^{i,b2b}(t)),&t\in[t_{j,N},t_{j,C})\end{cases}$ (9c)

Algorithm 1 The Energy-Aware Scheduling algorithm

1:function Schedule($x_{j}^{i,can},x_{j-1}^{i,com},e^{i,res}_{j}$)

2: GapVio = gware($x_{j}^{i,can},x_{j-1}^{i,com}$)

3: if GapVio == True then

4: return

5: end if

6: eware($x_{j}^{i,can},x_{j-1}^{i,com},e^{i,res}_{j}$)

7:end function

where
$\pi^{i}_{r}:\mathcal{Z}_{r}^{i}\times\mathcal{X}_{r}^{i}\rightarrow\mathcal{U}_{r}^{i}$
is a control policy to track the portion of the nominal trajectory and the b2b
trajectory for $i^{th}$ rechargeable robot. Figure 2 illustrates the
generation of candidate trajectories for a scenario involving three
rechargeable and one mobile charging robot.

###  III-D Reserve energy

While computing the reserve energy, we make the following assumption:

######  Assumption 3.

When the returning rechargeable robot reaches rendezvous point $x^{c}_{rp}$ at
$t_{j,C}$, there exists a landing controller
$\pi^{i}_{l}:[t_{j,C},t_{j,R})\times\mathcal{X}_{r}^{i}\rightarrow\mathcal{U}$
that guides the rechargeable robot to the mobile charging robot.

We ensure that the rechargeable robot returns to the rendezvous point with
enough energy for the worst-case landing. The worst-case landing refers to the
robot traveling from the rendezvous point $x^{c}_{rp}$ to the worst-case
estimate $\hat{x}^{c}_{w}(t_{j,R})$ computed in Section III-B. Before
computing the worst-case reserve energy, we first solve the following optimal
control problem to compute the worst-case landing trajectory of the
rechargeable robot system.

$\displaystyle\min_{\chi^{i}(t),u^{i}(t)}\ $
$\displaystyle\int_{t_{j,C}}^{t_{j,R}}\left\|x^{i}(t)-\hat{x}^{c}_{w}(t_{j,C})\right\|_{\mathbf{Q}}^{2}+\left\|u^{i}(t)\right\|_{\mathbf{R}}^{2}dt$
(10a) s.t. $\displaystyle\chi^{i}(t_{j,C})=\chi_{j}^{i,can}(t_{j,C})$ (10b)
$\displaystyle\dot{\chi}=f_{r}^{i}(\chi^{i},u^{i})$ (10c) $\displaystyle
x^{i}(t_{j,R})=\hat{x}^{c}_{w}(t_{j,R})$ (10d)

Using the solution from (10), we compute the reserve energy for each
rechargeable robot as follows:

$\displaystyle e_{j}^{i,res}=e^{i}(t_{j,R})-e^{i}(t_{j,C})\quad\forall
i\in\mathcal{R}$ (11)

###  III-E Energy-aware Scheduling

Given the candidate trajectory and the reserve energy for each rechargeable
robot $i\in\mathcal{R}$, we check if the minimum SoC constraint (4e) and the
minimum gap constraint (4f) are satisfied throughout the candidate trajectory.
The overall algorithm described in Algorithm 1 consists of the two sub-
routines: gware (gap-aware) and eware (energy-aware).

Algorithm 2 The gware algorithm

1:function gware($x_{j}^{i,can},x_{j-1}^{i,com}$)

2: Sort $x_{j}^{i,can}$ based on $T_{F}^{i}$

3: for $k\in\textbf{sorted list}$
$\mathcal{R}^{\prime}=\\{1^{\prime},2^{\prime},\cdots,N^{\prime}-1\\}$ do

4: $G_{f}^{k}=(T_{F}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta})$

5: if $G_{f}^{k}==$ False then

6: $x_{j}^{0^{\prime},com}\leftarrow x_{j-1}^{0^{\prime},com}$

7: $x_{j}^{k,com}\leftarrow x_{j}^{k,can}\quad\forall
k\in\\{1^{\prime},2^{\prime},\cdots,N^{\prime}-1\\}$

8: Initiate landing for the $0^{{}^{\prime}th}$ robot at $t_{j,C}$

9: return True

10: end if

11: end for

12:end function

Algorithm 3 The eware algorithm

1:function eware($x_{j}^{i,can},x_{j-1}^{i,com},T^{i}_{R,j},e^{i,res}_{j}$)

2: for $i\in\\{0,1,\cdots,N-1\\}$ do

3: if $e^{i}(t)\geq e^{i,res}_{j}\ \forall t\in[t_{j},t_{j,C}]$ then

4: $x_{j}^{i,com}\leftarrow x_{j}^{i,can}$

5: else

6: $x_{j}^{i,com}\leftarrow x_{j-1}^{i,com}$

7: Initiate landing for the $i^{th}$ robot at $t_{j,C}$

8: end if

9: end for

10:end function

####  III-E1 Gap-aware (gware)

gware described in Algorithm 2 checks if the rechargeable robots would
continue to have the required gap of $T_{ch}+T_{\delta}$ seconds between their
expected return to the charging station if the candidate trajectories were
committed.

Figure 3: This figure illustrates the induction step of the 1.

(Lines 2-4) The rechargeable robots are initially sorted based on their
remaining battery time, denoted as $T_{F,j}^{i}$. Subsequently, we check the
Gap flag condition for each $k^{th}$ rechargeable robot in the ordered list
w.r.t to the first rechargeable robot ($0^{{}^{\prime}th}$ robot in the sorted
list), i.e.,

$\displaystyle G_{f}^{k}=(T_{F,j}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta}).$ (12)

This flag ensures that a minimum gap of $k(T_{ch}+T_{\delta})$ is maintained
between the first and the $k^{th}$ rechargeable robots in the ordered list,
treating the first robot as if it were returning. For example, the minimum
desired gap between the first and third rechargeable robot is
$2(T_{ch}+T_{\delta})$. Satisfaction of the gap flag condition at the $j^{th}$
iteration implies that rechargeable robots are estimated to have at least
$T_{ch}+T_{\delta}$ of the gap between their expected returns over the time
interval $[t_{j},t_{j,R})$, i.e. $\forall
t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$:

$\displaystyle(T_{F,j}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta})$ (13)
$\displaystyle\implies|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|>T_{ch}+T_{\delta}$
$\displaystyle\forall t\in[t_{j},t_{j,R})$ (14)

(Lines 5-7) If any gap flags are false, the committed trajectory of the first
rechargeable robot in the sorted list (with the shortest remaining battery
time) remains unchanged, and it returns to the charging station. Meanwhile,
the candidate trajectories are committed for the subsequent rechargeable
robots in the sorted list. By bringing back the first robot in the sorted list
early, we ensure that the second robot in the sorted list has at least
$T_{ch}+T_{\delta}$ seconds of battery time before it returns to the charging
robot.

####  III-E2 Energy-aware (eware)

If no gap flag violations occur, indicating that all rechargeable robots have
sufficient gaps between their expected return for recharging, we proceed to
check if each robot has adequate energy to continue the mission using eware.

(Lines 3-6) We assess whether each rechargeable robot can reach the charging
station without depleting its energy below the reserve level while following
the candidate trajectory. We refer to this condition as the Reserve SoC
Condition:

$\displaystyle e^{i}(t)>e^{i,res}_{j}\ \forall t\in[t_{j},t_{j,C}].$ (15)

If successful, the candidate trajectory replaces the current committed
trajectory.

Figure 4: Results from the Multi-Agent Energy-Aware Persistent Ergodic Search
Case study

######  Theorem 1.

Suppose that at the first iteration i.e., $j=0$, the Gap flag condition (12)
and the Reserve SoC condition (15) are satisfied. Therefore, the constraints
(4e) and (4f) are satisfied for $[t_{0},t^{i}_{0,R})$. Following this, if
$\quad\forall i\in\mathcal{R}$, the committed trajectories
$x_{j}^{i,com}=x^{i}([t_{j},t_{j,C}];t_{j},x^{i}_{j})$ are determined using
the Algorithm 1, then the constraints (4e) and (4f) are satisfied $\forall
t\in[t_{j},t_{j-1,C})$, $\forall j\in\mathbb{Z}_{+}$.

###### Proof.

The proof, inspired by [29, Thm. 1], uses induction.

#### Base Case

At the time $t_{1}$ and iteration $j=1$, since both Gap flag condition (12)
and the Reserve SoC condition (15) are true, the candidate trajectories are
committed for all rechargeable robots i.e. $\forall i\in\mathcal{R}$ and
$\forall t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$

$\displaystyle x_{1}^{i,com}(t)\leftarrow x_{1}^{i,can}(t)\quad\forall
t\in[t_{1},t_{1,C})$
$\displaystyle\implies\begin{cases}(T_{F,1}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta})\quad\forall
k\in\mathcal{R}^{\prime}\\\ e^{i}(t)>e^{res}_{1}\quad\forall
t\in[t_{1},t_{1,C})\end{cases}$
$\displaystyle\implies\begin{cases}|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|>T_{ch}+T_{\delta}&\forall
t\in[t_{1},t_{1,R})\\\ e^{i}(t)>e_{min}^{i}\quad&\forall
t\in[t_{1},t_{1,R})\end{cases}$

Since $t_{1,R}>t_{1,C}>t_{0,C}$, the claim holds.

#### Induction Step

Suppose the claim is true for some $j\in\mathbb{Z}_{+}$. We show that the
claim is true for $j+1$. The solution at the iteration $j+1$ has two
definitions illustrated in Figure 3:

#### Case 1

When candidate trajectories for all rechargeable robots are valid, i.e.
$\forall i\in\mathcal{R}$ and and $\forall
t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$

$\displaystyle x_{j+1}^{i,com}(t)\leftarrow x_{j+1}^{i,can}(t)\quad\forall
t\in[t_{j+1},t_{j+1,C})$
$\displaystyle\implies\begin{cases}(T_{F,j+1}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta})\quad\forall
k\in\mathcal{R}^{\prime}\\\ e^{i}(t)>e^{res}_{j+1}\quad\forall
t\in[t_{j+1},t_{j+1,C})\end{cases}$
$\displaystyle\implies\begin{cases}|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|>T_{ch}+T_{\delta}&\forall
t\in[t_{j+1},t_{j+1,R})\\\ e^{i}(t)>e_{min}^{i}\quad&\forall
t\in[t_{j+1},t_{j+1,R})\end{cases}$

Since $t_{j+1,R}>t_{j+1,C}>t_{j,C},\forall i\in\mathcal{R}$ the claim holds.

#### Case 2

This case corresponds to the scenario when the first robot
($0^{{}^{\prime}th}$ robot in the ordered list) returns either due to Gap flag
condition being false, or because of the violating Reserve SoC condition,
i.e.,

$\displaystyle x_{j+1}^{0^{\prime},com}(t)\leftarrow
x_{j}^{0^{\prime},com}(t)\quad\forall t\in[t_{j+1},t_{j,C}).$

The candidate trajectories are committed for the remaining robots, i.e.
$\forall k\in\mathcal{R}^{^{\prime}}\backslash\\{0^{\prime}\\}$ and $\forall
t^{i_{1}}_{s,m_{1}},t^{i_{2}}_{s,m_{2}}\in\mathcal{T}$

$\displaystyle x_{j+1}^{k,com}\leftarrow x_{j+1}^{k,can}\quad\forall
t\in[t_{j+1},t_{j+1,C})$
$\displaystyle\implies\begin{cases}(T_{F,j+1}^{k}-T_{L}-T_{C})>k(T_{ch}+T_{\delta})\\\
e^{k}(t)>e^{res}_{j+1}\quad\forall t\in[t_{j+1},t_{j+1,C})\end{cases}$
$\displaystyle\implies\begin{cases}|t^{i_{1}}_{s,m_{1}}-t^{i_{2}}_{s,m_{2}}|>T_{ch}+T_{\delta}&\forall
t\in[t_{j+1},t_{j+1,R})\\\ e^{k}(t)>e_{min}^{k}\quad&\forall
t\in[t_{j+1},t_{j+1,R})\end{cases}$

Since $t_{j+1,R}>t_{j,C}$, the claim holds.

∎

Given that the constraints (4e) and (4f) are met for at least over the
interval $[t_{j},t_{j-1,C})$ $\forall j\in\mathbb{Z}_{+}$ , and
$t_{j}<t_{j-1,N}<t_{j-1,C}$, the constraints (4e) and (4f) are satisfied over
$[t_{0},\infty)$.

##  IV Results & Discussion

In this section, we evaluate meSch through simulation experiments and
demonstrate its working on physical hardware. We use quadrotors modeled with
nonlinear dynamics described in [31, Eq. (10)] as the rechargeable robots and
rovers modeled with unicycle models as the mobile charging robots. We assume
instantaneous recharging, i.e., $T_{ch}=0.0$ seconds with the buffer time of
$T_{\delta}=15.0$ seconds. Moreover, we set $T_{N}=2.0$, $T_{B}=10.0$,
$T_{C}=12.0$ and $T_{R}=18.0$. We use the same values for the simulation and
hardware experiments.

To generate back-to-base (b2b) trajectories, we solve the problem (7) using
model predictive control (MPC) with the reduced linear quadrotor dynamics
proposed by [31]. We use Linear Quadratic Regulator (LQR) with a reduced
linear quadrotor model as the landing controller. The committed trajectories
are generated at 0.5 Hz (i.e., $T_{E}=2.0$ seconds) and tracked at 20.0 Hz
with zero-order hold. Furthermore, we use the Runge-Kutta 4th order (RK4)
integration scheme. We evaluate meSch through different case studies and
discuss its multi-agent architecture, which offers algorithmic computational
efficiency and scalability.

####  IV-1 Multi-Agent Energy-Aware Persistent Ergodic Search

We evaluate meSch by simulating a hypothetical scenario where four quadrotors
and one rover persistently explore a coverage domain of $10\times 10$ meters.
Nominal trajectories for all robots are generated using the Projection
Trajectory Optimization (PTO) method [32]. PTO iteratively replans
trajectories with a horizon of $T_{H}=30.0$, ensuring validity for at least
$T_{C}=12.0$ seconds. Constant discharge dynamics $\dot{e}=-k_{d}$ are used
for all quadrotors, with $k_{d}=0.667$. Coverage objectives and environmental
characteristics are not discussed for brevity. Figure 4 summarizes the case
study results. Figure 4 (A) depicts only one quadrotor at the charging station
at any given time, with no collisions observed between quadrotors during
visits. Collision prevention is achieved by tuning the $T_{\delta}$ parameter,
ensuring no overlapping visits. Figure 4 (B) plots the SoC evolution over time
for two cases: with the stochastic mobile charging robot and with the
deterministic mobile charging robot. With the stochastic robot, quadrotors’
SoC levels never drop below the reserve energy level. Without noise in the
mobile charging dynamics, the reserve energy is zero, allowing SoC levels to
drop below it. Lastly, Figure 4 (C) plot shows that nominal ergodic trajectory
(in red) is only affected when quadrotor 1 decides to return for recharging.

Figure 5: These plots show results for the scenarios when four quadrotors have
different SoC capacities and different discharge rates. The plots validate
that quadrotors always maintain the minimum of $T_{c}+T_{\delta}$ gap while
visiting the charging station.

####  IV-2 Comparison of performance with different discharge rates and
different SoC capacities

Here, we evaluate the proposed energy-aware scheduling algorithm across
scenarios with four quadrotors exhibiting varying battery discharge rates and
SoC capacities. Results are summarized in Figure 5.

In scenario (A), the first quadrotor returns with almost $42\%$ SoC, enabling
subsequent quadrotors to maintain the $T_{ch}+T_{\delta}$ gap, resulting in
quadrotor 4 returning with approximately $3\%$ SoC. Additionally, in scenarios
(B) and (D), quadrotors 1 and 2 visit the charging station with a gap greater
than $T_{ch}+T_{\delta}$, indicating battery depletion rather than risk of gap
violation. In an ablation study, scenario (E) reveals that removing the gware
sub-routine from meSch leads to quadrotors visiting the charging robot
simultaneously, violating (4f). In scenario (F), without eware, quadrotors
deplete their energy before returning to the charging robot, though without
gap violations.

####  IV-3 Hardware demonstration

We present an initial demonstration of the proposed framework utilizing 1 Aion
Rover as a mobile charging robot and 2 Crazyflie 2.0 as rechargeable robots,
as shown in Figure 6. The experiments were conducted in an indoor lab equipped
with 17 VICON cameras for precise state estimation. The trajectories were
generated centrally at a DELL G15 (Intel i7, 32 GB) at 0.5 Hz and tracked at
20 Hz. We used the same values for the time horizons in the experiment as
those used in the simulations. Furthermore, in this experiment, we employed
the same constant discharge model for Crazyflies as used in simulations. The
results, as shown in Figure 6 and Figure 7, validate the findings and
demonstrate that the Crazyflies successfully return to the mobile charging
rover before their energy depletes according to the discharge model.
Additionally, they maintain the $T_{ch}+T_{\delta}$ duration while visiting
the mobile charging robot.

####  IV-4 Computational efficiency and scalability

This section outlines the computational costs associated with the proposed
module, meSch. Implementing the module within a distributed scheme is crucial
for enhancing computational efficiency. The primary computational overhead in
meSch arises from generating candidate trajectories, which involves solving
the (7) problem and numerically integrating it through system non-linear
dynamics. On average, solving (7) consumes approximately 150 ms, with an
additional 30 ms required for the forward integration process.

Figure 6: Physical hardware demonstration of meSch. [Video:
https://youtu.be/V8SryQhNdXE]  Figure 7: Plots from the experimental trials.
Left: SoC over time. Right: Distance to Charging station over time.

To support real-time applications, each rechargeable robot generates its
candidate trajectories either on board or on a separate node within the
simulation framework. This distributed approach efficiently handles trajectory
generation for N rechargeable robots. Subsequently, these candidate
trajectories are transmitted to the central node, where the scheduling
algorithm is executed. The time complexity of the scheduling algorithm is
$\mathcal{O}(N\log N)$, primarily due to the sorting function in line 2 of
Algorithm 2. Consequently, the proposed method scales with $\mathcal{O}(N\log
N)$, where $N$ is the number of rechargeable robots.

Finally, we demonstrate the scalability of the proposed method through two
scenarios. In the first scenario, we deploy 33 quadrotors for persistent
formation control. A high-level trajectory planner designs trajectories for
the robots to maintain the formation, which are then iteratively filtered by
meSch at a frequency of 0.5 Hz. We assess the performance of this setup under
both static and mobile charging conditions. Additionally, we present another
scenario to showcase the robustness of our method to nominal trajectories.
Here, 30 quadrotors flying at varying altitudes persistently follow irregular
nominal trajectories. Unlike the first scenario, these trajectories lack
symmetry, and some require quadrotors to travel far from the charging station.
Nonetheless, we validate that, regardless of whether the charging station is
static or mobile, the quadrotors adeptly maintain persistence in following the
nominal trajectories. We direct readers to Figure 8 for visuals of both
scenarios and find links to the full simulation videos for all four cases
within the figure.

Figure 8: Two scenarios are tested with static and mobile charging stations to
validate scalability. For full simulation videos, please click the link for
respective cases: [Case (A.1): https://youtu.be/1AVFzKUGCXw], [Case (B.1):
https://youtu.be/zpXPfNlhFmo], [Case (A.2): https://youtu.be/OQSIq_-QQk4], and
[Case (B.2): https://youtu.be/g1g8UiNEhho]

##  V Conclusion

In this work, we introduced a framework meSch to ensure the exclusive use of a
mobile charging station by the robots doing a persistent task. Through formal
analysis and case studies, we demonstrate that the proposed method achieves
recharge scheduling for robots of nonlinear dynamics that may have different
battery capacities and discharge rates. Additionally, we show that meSch
robustly estimates the rendezvous points while incorporating the uncertainty
in the charging station’s position. In future research, we plan to extend this
work to address challenges related to environmental disturbances and
communication delays among robots.

## References

  * [1] Y. Li, Y. Gao, S. Yang, and Q. Quan, “Swarm robotics search and rescue: A bee-inspired swarm cooperation approach without information exchange,” in _2023 IEEE International Conference on Robotics and Automation (ICRA)_ , 2023, pp. 1127–1133.
  * [2] S. Manjanna, A. Q. Li, R. N. Smith, I. Rekleitis, and G. Dudek, “Heterogeneous multi-robot system for exploration and strategic water sampling,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 4873–4880.
  * [3] R. E. Todd, “Export of middle atlantic bight shelf waters near cape hatteras from two years of underwater glider observations,” _Journal of Geophysical Research: Oceans_ , vol. 125, no. 4, 2020.
  * [4] S. Soldan, J. Welle, T. Barz, A. Kroll, and D. Schulz, “Towards autonomous robotic systems for remote gas leak detection and localization in industrial environments,” in _Field and Service Robotics: Results of the 8th Int. Conference_. Springer, 2014, pp. 233–247.
  * [5] W. Bentz and D. Panagou, “Energy-aware persistent coverage and intruder interception in 3d dynamic environments,” in _2018 Annual American Control Conference (ACC)_ , 2018, pp. 4426–4433.
  * [6] G. Notomista, C. Pacchierotti, and P. R. Giordano, “Online robot trajectory optimization for persistent environmental monitoring,” _IEEE Control Systems Letters_ , vol. 6, pp. 1472–1477, 2022.
  * [7] K. Sundar and S. Rathinam, “Algorithms for routing an unmanned aerial vehicle in the presence of refueling depots,” _IEEE Transactions on Automation Science and Engineering_ , vol. 11, no. 1, pp. 287–294, 2014.
  * [8] M. Wei and V. Isler, “Coverage path planning under the energy constraint,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_ , 2018, pp. 368–373.
  * [9] K. B. Naveed, D. Agrawal, C. Vermillion, and D. Panagou, “Eclares: Energy-aware clarity-driven ergodic search,” in _2024 IEEE International Conference on Robotics and Automation (ICRA)_ , 2024.
  * [10] G. Notomista, “Resilience and energy-awareness in constraint-driven-controlled multi-robot systems,” in _2022 American Control Conference (ACC)_ , 2022, pp. 3682–3687.
  * [11] G. Notomista, C. Pacchierotti, and P. R. Giordano, “Multi-robot persistent environmental monitoring based on constraint-driven execution of learned robot tasks,” in _2022 International Conference on Robotics and Automation (ICRA)_ , 2022, pp. 6853–6859.
  * [12] G. Notomista, S. F. Ruf, and M. Egerstedt, “Persistification of robotic tasks using control barrier functions,” _IEEE Robotics and Automation Letters_ , vol. 3, no. 2, pp. 758–763, 2018.
  * [13] A. B. Asghar, S. Sundaram, and S. L. Smith, “Multi-robot persistent monitoring: Minimizing latency and number of robots with recharging constraints,” 2023.
  * [14] M. Kenzin, I. Bychkov, and N. Maksimkin, “Coordinated recharging of heterogeneous mobile robot teams during continuous large scale missions,” in _2020 7th International Conference on Control, Decision and Information Technologies (CoDIT)_ , vol. 1, 2020, pp. 745–750.
  * [15] L. Liu and N. Michael, “Energy-aware aerial vehicle deployment via bipartite graph matching,” in _2014 International Conference on Unmanned Aircraft Systems (ICUAS)_ , 2014, pp. 189–194.
  * [16] B. Li, S. Patankar, B. Moridian, and N. Mahmoudian, “Planning large-scale search and rescue using team of uavs and charging stations,” in _2018 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR)_ , 2018, pp. 1–8.
  * [17] A. Seewald, C. J. Lerch, M. Chancán, A. M. Dollar, and I. Abraham, “Energy-aware ergodic search: Continuous exploration for multi-agent systems with battery constraints,” 2024.
  * [18] J. Xu, J. Wang, and W. Chen, “An efficient recharging task planning method for multi-robot autonomous recharging problem,” in _2019 IEEE International Conference on Robotics and Biomimetics (ROBIO)_ , 2019, pp. 1839–1844.
  * [19] W. Bentz, T. Hoang, E. Bayasgalan, and D. Panagou, “Complete 3-d dynamic coverage in energy-constrained multi-uav sensor networks,” _Autonomous Robots_ , vol. 42, pp. 825–851, 2018.
  * [20] H. Fouad and G. Beltrame, “Energy autonomy for robot systems with constrained resources,” _IEEE Transactions on Robotics_ , vol. 38, no. 6, pp. 3675–3693, 2022.
  * [21] T. Gao and S. Bhattacharya, “Multirobot charging strategies: A game-theoretic approach,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 3, pp. 2823–2830, 2019.
  * [22] N. Karapetyan, A. B. Asghar, A. Bhaskar, G. Shi, D. Manocha, and P. Tokekar, “Ag-cvg: Coverage planning with a mobile recharging ugv and an energy-constrained uav,” 2023.
  * [23] N. Kingry, Y.-C. Liu, M. Martinez, B. Simon, Y. Bang, and R. Dai, “Mission planning for a multi-robot team with a solar-powered charging station,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2017, pp. 5233–5238.
  * [24] T. X. Lin, E. Yel, and N. Bezzo, “Energy-aware persistent control of heterogeneous robotic systems,” in _2018 Annual American Control Conference (ACC)_ , 2018, pp. 2782–2787.
  * [25] N. Mathew, S. L. Smith, and S. L. Waslander, “Multirobot rendezvous planning for recharging in persistent tasks,” _IEEE Transactions on Robotics_ , vol. 31, no. 1, pp. 128–142, 2015.
  * [26] A. Couture-Beil and R. T. Vaughan, “Adaptive mobile charging stations for multi-robot systems,” in _2009 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2009, pp. 1363–1368.
  * [27] X. Lin, Y. Yazıcıoğlu, and D. Aksaray, “Robust planning for persistent surveillance with energy-constrained uavs and mobile charging stations,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4157–4164, 2022.
  * [28] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” _IEEE Trans. on Automatic Control_ , vol. 62, no. 8, pp. 3861–3876, 2017.
  * [29] D. Agrawal, R. Chen, and D. Panagou, “gatekeeper: Online safety verification and control for nonlinear systems in dynamic environments,” in _2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2023, pp. 259–266.
  * [30] A. Gelb _et al._ , _Applied optimal estimation_. MIT press, 1974.
  * [31] B. E. Jackson, K. Tracy, and Z. Manchester, “Planning with attitude,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 5658–5664, 2021.
  * [32] L. Dressel and M. J. Kochenderfer, “Tutorial on the generation of ergodic trajectories with projection-based gradient descent,” _IET Cyper-Phys. Syst.: Theory & Appl._, vol. 4, no. 2, pp. 89–100, 2019. [Online]. Available: https://doi.org/10.1049/iet-cps.2018.5032