<figure><img src="image_1.png" /></figure>

HAL
open science

# Online Robot Trajectory Optimization for Persistent Environmental Monitoring

Gennaro Notomista, Claudio Pacchierotti, Paolo Robuffo Giordano

► To cite this version:

Gennaro Notomista, Claudio Pacchierotti, Paolo Robuffo Giordano. Online Robot Trajectory Optimization for Persistent Environmental Monitoring. IEEE Control Systems Letters, 2022, 6, pp.1472-1477. 10.1109/LCSYS.2021.3110940. hal-03335162

HAL Id: hal-03335162

https://inria.hal.science/hal-03335162

Submitted on 6 Sep 2021

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.
# Online Robot Trajectory Optimization for Persistent Environmental Monitoring

Gennaro Notomista, Member, IEEE, Claudio Pacchierotti, Senior Member, IEEE, and Paolo Robuffo
Giordano Senior Member, IEEE

**Abstract**— This paper presents a control-theoretic approach for trajectory optimization of mobile robots suitable for environmental monitoring. The method is based on the optimization of the Constructability Gramian in order to maximize the information collected while traversing a state trajectory. The maximization of the collected information is combined with energy constraints to define an optimization-based controller that achieves persistent environmental monitoring. The results of its application to the estimation of the concentration of a diffusing gas using a mobile robot are shown in simulation.

**Index Terms**— Robotics, Energy systems, Constrained control

## I. INTRODUCTION

The use of robotic systems for environmental monitoring is becoming more and more common thanks to their suitability to collect spatial and temporally disperse data [1]. However, the longer the time horizon over which robots need to remain operational to collect environmental data, the more challenging their deployment becomes. In particular, energy management and control is paramount when autonomous, untethered robotic platforms are considered [2].

In this paper, we propose an online trajectory optimization strategy for mobile robots suitable for persistent environmental monitoring tasks. The approach consists of two main components: (i) Trajectory optimization to maximize the gathered information, framed as an active sensing problem [3], and (ii) Energy control using physics-based robot battery model implemented using control barrier functions [4]. These two components are combined in a single optimization-based controller which computes both the optimized trajectory and the control input required by the robot to track it.

Robots deployed for persistent environmental monitoring have been considered in [5], where a spatial information field has to be swept by the robots whose velocity is controlled over a predefined trajectory in space. The employment of autonomous agents for environmental monitoring has been recognized to be particularly useful for source localization applications [6], [7]. In [8], mobile sensors are controlled to move in order to maximize the information collected required

to identify the source of an advective-diffusive environmental field model, implementing, as a matter of fact, an active sensing control strategy.

Active sensing has been studied under different perspectives (see, e.g., [9]–[12]). In [3], the authors show how a norm of the so-called Constructability Gramian (CG) is an appropriate measure of the amount of information gathered along a given trajectory of the state of a system. Leveraging this insight, in this paper we develop a persistent environmental monitoring strategy where the estimation of environmental fields is to be achieved under limited energy resources, which characterize the mobile robotic platforms employed for such tasks. Moreover, compared to empirical methods devised to maximize robot deployment time [13] or minimize energy expenditure [14], we do so in a provable fashion by employing control barrier functions to enforce energy constraints.

The main contributions of this paper with respect to existing approaches are: (i) Extending the control-theoretic method proposed in [3] to online robot trajectory planning for environmental monitoring; (ii) Considering the limited energy availability on the robots using an accurate battery model; (iii) Combining (i) and (ii) in an optimization-based controller to achieve a computationally efficient persistent environmental monitoring algorithm.

## II. ACTIVE SENSING FOR ENVIRONMENTAL MONITORING

This section introduces the active sensing problem which will be extended to cover environmental monitoring scenarios. A control-theoretic approach based on the CG will be employed and combined with energy control to achieve persistent environmental monitoring. The solution proposed in this paper builds on the ideas originally proposed in [3] which are briefly recalled in the next subsection for the reader's convenience.

### A. Active Sensing

In active sensing tasks, robot trajectories are planned with the objective of maximizing the information gathered by the robots during motion. In order to quantify the gathered information, we start by defining models for robot motion and sensor measurement, as follows:

$$ \begin{cases} \dot{x} = f(x, u) + \epsilon_x \\ y = m(x) + \epsilon_y. \end{cases} \qquad (1) $$

<footnote>The authors are with CNRS, Univ Rennes, Inria, IRISA, Rennes, France {gennaro.notomista, claudio.pacchierotti, prg}@irisa.fr
This work was sponsored by the ANR project ANR-20-CHIA-0017 "MULTISHARED"</footnote>
Here, $f: \mathbb{R}^{n_x} \times \mathbb{R}^{n_u} \to \mathbb{R}^{n_x}$ is a Lipschitz continuous vector field modeling the robot dynamics. The measurement model is given by $m: \mathbb{R}^{n_x} \to \mathbb{R}^{n_u}$. In the context of persistent environmental monitoring considered in this paper, measurements $y$ will be leveraged to produce an estimate of the state $x$ using an extended Kalman filter. The system and measurement noise are modeled by zero-mean Gaussian distributions with covariance matrices $Q$ and $R$, respectively, i.e. $\epsilon_x \sim N(0, Q)$ and $\epsilon_y \sim N(0, R)$.

In [3], the authors propose a solution to the active sensing problem consisting in optimizing the state trajectory in order to maximize a norm of the CG for system (1), which is defined over a time horizon $[t_s, t_f]$ as:

$$ \mathcal{G}_c(t_s, t_f) = \int_{t_s}^{t_f} \Phi(\tau, t_f)^T C(\tau)^T W(\tau) C(\tau) \Phi(\tau, t_f) d\tau, \quad (2) $$

where $\Phi(\tau, t_f) \in \mathbb{R}^{n_x \times n_x}$ is the transition matrix of the dynamical system $\dot{x} = f(x, u)$, $C(\tau) = \nabla_x m(x(\tau))^T$, and $W$ is a weight matrix set to $R^{-1}$, the inverse of the covariance matrix of the measurement noise $\epsilon_y$ [3]. This way, a norm of the CG is a suitable metric for quantifying the information gathered while moving along the state trajectory $\hat{x}: [t_s, t_f] \to \mathbb{R}^{n_x}$.

The infinite-dimensional trajectory optimization can be reduced to a finite-dimensional optimization problem by parameterizing the state trajectory $x(t)$, $t \in [t_s, t_f]$ using a finite number of parameters $x_c \in \mathbb{R}^{n_{xc}}$. This can be done, for instance, by considering polynomial (such as cubic splines and B-splines) state trajectories. Then, the following gradient-ascent strategy can be employed to optimize the trajectory so to maximize the information gathered:

$$ \dot{x}_c = K \nabla_{x_c} \mathcal{G}_c(t_s, t_f), \quad (3) $$

$K$ being a positive gain. We refer to [3] for the details about the evaluation of the gradient of the CG $\mathcal{G}_c(t_s, t_f)$ with respect to the trajectory parameters $x_c$.

## B. Environmental Monitoring as Active Sensing

In environmental monitoring tasks, robots are deployed to estimate an environmental field, such as temperature and gas concentration maps. In this paper, we assume the field of interest can be parameterized by a vector $\theta \in \mathbb{R}^{n_\theta}$. Then, the robot and measurement model (1) can be extended as follows:

$$ \left\{ \begin{array}{l} \dot{x} = f(x, u) + \epsilon_x \\ \dot{\theta} = g(\theta, t) + \epsilon_\theta \\ y = m(x, \theta) + \epsilon_y \end{array} \right. . \quad (4) $$

The dynamics of the environmental field—which need not be static—are modeled by the vector field $g: \mathbb{R}^{n_\theta} \times \mathbb{R} \to \mathbb{R}^{n_\theta}$ assumed to be Lipschitz continuous in the first argument, $\theta$, and continuous in its second argument, $t$. This choice allows us to consider dynamically evolving environmental fields (such as gas concentration, solar light intensity, and air properties), and it is suitable to represent discretized solutions of partial differential equations modeling an environmental field. The formulation in (4) allows us to treat environmental monitoring

tasks as instances of active sensing. Following what has been proposed in [3], a trajectory for $x$ can be planned in order to maximize the information gathered through the measurements $y$ required to estimate $\theta$.

**Remark 1.** In the model (4), the state is comprised of both $x$ and $\theta$. However, the control input $u$ influences directly only the state of the robot, $x$, while $\theta$ evolves independently. Therefore, a state trajectory can be planned considering just the components $x$ of the state of the system (4). It is worth, however, pointing out that $\theta$ is nevertheless taken into account in the state trajectory optimization through the gradient of $\mathcal{G}_c(t_s, t_f)$ which does depend on $\theta$.

The formulation in (4) allows us to execute environmental monitoring tasks by optimizing the state trajectory. In the next section, we will present the battery charge control as a way of rendering environmental monitoring tasks persistent, so to allow robots to execute them over long time horizons.

## III. BATTERY CHARGE CONTROL

Environmental monitoring tasks may take place over long time horizons depending on the nature of the phenomenon of interest that needs to be monitored. In this paper, we present an approach to perform energy-constrained environmental monitoring which consists of an optimization-based strategy for trajectory optimization constrained by the limited availability of energy that robots can carry around.

In [15] and [16], the persistification of generic robotic tasks is considered, through which the energy of the robots executing the tasks is controlled to remain about a lower bound. However, the considered battery model is conservative as it always takes into account the worst-case scenario of maximum discharge rate that the battery of the robot can undergo. In this section, we refine the persistification strategy by introducing a more faithful energy model that describes the dynamics of the state of charge (SOC) of a battery. Then, we introduce integral control barrier functions (I-CBFs) as a way of enforcing energy constraints. The SOC dynamic model will be used in conjunction with I-CBFs in order to ensure that the battery of the robots executing a task never depletes—i.e. the SOC never falls below a minimum threshold—while executing a task of interest, in this case the trajectory optimization task to realize environmental monitoring through active sensing.

### A. State of Charge Dynamics

The state of charge is used to describe the remaining capacity of a battery, and it is therefore a very important parameter to take into account while designing energy-aware control strategies [17]. Its accurate knowledge allows us to design algorithms to protect batteries from overdischarge and overcharge, increasing their life. However, batteries store chemical energy, which cannot be directly measured and has to be, therefore, estimated. The SOC estimation still remains a fundamental challenge, as it hinges on battery models which, in turn, depend on many parameters [18]. A survey of methods which have been proposed to estimate the SOC of batteries can be found in [19], [20]. In the following, we present
the basic SOC dynamic model (found, e.g., in [21]) which can be employed to render the environmental monitoring task persistent, by preventing robot batteries from depleting.

Let $e(t)$ denote the value of the SOC of the robot battery at time $t$, $C$ its rated capacity, $\eta$ its Coulombic efficiency, and $i(t)$ the current flowing from the battery—$i(t) > 0$ if the current is flowing out of the battery at time $t$, $i(t) < 0$ if it is flowing in. Then, the value of $e(t)$ can be evaluated as follows:

$$e(t) = e(t_0) - C^{-1} \int_{t_0}^{t} \eta i(t) dt, \quad (5)$$

where, in general, the efficiency $\eta$ can be a function of the current $i(t)$, the temperature, the SOC itself, as well as the state of health (SOH) of the battery [21].

In a large variety of robotic applications, it is reasonable to assume that the current supplied by the battery is proportional to the magnitude of the control input $\|u\|$ supplied to the robot. This holds, for instance, in cases where electric motors and actuators—for which exerted torque is proportional to absorbed current—are employed for motion and locomotion purposes [22]. Therefore, we can model the relation between current and robot input as $i(t) = \alpha_i (\|u\|^2)$, where $\alpha_i$ is a class $\mathcal{K}$ function.

Starting from (5), and using this assumption, yields the following SOC dynamic model for the battery of robot $i$:

$$\dot{e} = -\eta C^{-1} \alpha_i (\|u\|^2). \quad (6)$$

## B. Integral Control Barrier Functions

The model of the battery SOC dynamics in (6) depends on the control input $u$, and in general it does so in a non-control-affine fashion. As such, a straightforward application of CBF-based approaches would result in differential constraints which are not affine in $u$ [4]. As will be shown in the next section, affine input constraints are desirable in order to synthesize robot controllers under real-time constraints at high frequencies commonly used in the control loops of modern robotic platforms. To circumvent this issue, control dependent control barrier functions [23] or the more general formulation of integral control barrier functions [24], defined below, can be employed.

**Definition 2** ([24]). For the system $\dot{x} = f(x, u)$, with corresponding safe set $S \subset \mathbb{R}^{n_x} \times \mathbb{R}^{n_u}$ defined as the 0-superlevel set of a function $h: \mathbb{R}^{n_x} \times \mathbb{R}^{n_u} \to \mathbb{R}$ with $0$ a regular value: $S = \{(x, u) \in \mathbb{R}^{n_x} \times \mathbb{R}^{n_u} : h(x, u) \ge 0\}$. Then, $h$ is an integral control barrier function (I-CBF) if for any $(x, u) \in \mathbb{R}^{n_x} \times \mathbb{R}^{n_u}$ and $t \ge 0$, $p(x, u) = 0 \Rightarrow d(x, u, t) \le 0$, where the expressions of $p(x, u)$ and $d(x, u, t)$ are given in [24].

Analogously to CBFs, I-CBFs are amenable to ensure the forward invariance of subsets of the product space $\mathbb{R}^{n_x} \times \mathbb{R}^{n_u}$. Therefore, they can be used to ensure that the SOC of robot batteries never falls below a desired minimum threshold. In other words, we would like the set of values of SOC, $e$, above a desired minimum threshold to be forward invariant, which is equivalent to saying that we would like $e$ to be always greater than or equal to a desired minimum threshold. With this

above definition of I-CBFs, Theorem 1 in [24] gives sufficient conditions for the forward invariance of sets defined through an I-CBF.

In order to apply I-CBF-based control, the considered system $\dot{x} = f(x, u)$ has to be dynamically extended as follows [25]:

$$\left\{ \begin{aligned} \dot{x} &= f(x, u) \\ \dot{e} &= -\eta C^{-1} \alpha_i (\|u\|^2) \\ \dot{u} &= v, \end{aligned} \right. \quad (7)$$

where now $v$ is the control input, and $u$ becomes part of the state. With these dynamically extended dynamics, the following I-CBF can be used to keep the SOC value above a minimum threshold, denoted by $e_{\min}$:

$$h_x(x, e) = e - e_{\min} - \alpha_c (\|p(x) - p_c\|), \quad (8)$$

where $e$ is the value of the SOC of the battery of robot, $p(x)$ denotes the position of the robot, $p_c$ is the location of a charging station, and $\alpha_c$ is a monotonically increasing function. The term $\alpha_c (\|p(x) - p_c\|)$, then, represents the amount of energy required to reach the charging station.

The I-CBF $h_x$ has relative degree 2 with respect to the new control input $v$. Therefore, following the approach in [16], we can define an following auxiliary I-CBF, $h_u$, which explicitly depends on $u$, and thus has relative degree 1 with respect to $v$:

$$h_u(x, e, u) = \dot{h}_x(x, e, u) + \gamma_x (h_x(x, e)), \quad (9)$$

where $\gamma_x$ is a class $\mathcal{K}$ function. In the next section, it will be shown how to utilize the defined I-CBF in order to control the SOC of a robot battery while executing an environmental monitoring task through by solving the active sensing problem discussed in the previous section.

## IV. PERSISTENT ENVIRONMENTAL MONITORING

Combining the model required for active sensing tasks with the one that describes the dynamics of battery SOC, we can state the full model of a robot with state $x$, SOC $e$, control input $u$, dynamically extended control input $v$, and measurement model $m$, moving in an environment characterized by an environmental field parameterized by $\theta$, as follows:

$$\left\{ \begin{aligned} \dot{x} &= f(x, u) \\ \dot{e} &= -\eta C^{-1} \alpha_i (\|u\|^2) \\ \dot{u} &= v \\ \dot{\theta} &= g(\theta, t) \\ y &= m(x, \theta). \end{aligned} \right. \quad (10)$$

We are now ready to state the optimization-based controller used to realize persistent environmental monitoring, in the form of the following optimization nonlinear program (NLP):

$$\textbf{Main NLP} \quad (11)$$

$$\underset{v, \dot{x}_c, \delta}{\text{minimize}} \quad \|v\|^2 + \kappa \delta^2 \quad (11a)$$

$$\text{subject to} \quad \nabla_{x_c} V_{\text{sens}}^T \dot{x}_c \ge \phi(x_c) - \delta \quad (11b)$$

$$v = \psi(\dot{x}_c) \quad (11c)$$

$$\dot{h}_u(x, e, u, v) \ge -\gamma_u(h_u(x, e, u)), \quad (11d)$$
where the optimization variables are the dynamically extended control input, $v$, the parameters $x_c$ describing the state trajectory $\hat{x}$ to optimize, and a slack variable $\delta$, whose purpose will be explained in the following.

As discussed in Section II, environmental monitoring seen as an active sensing task can be executed by optimizing the parameters of the state trajectory, $x_c$, to plan a path that maximizes a norm of the CG, which, in this work, is chosen to be the trace. The advantage of the trace over other norms will be clear in the next section and is discussed in more detail in Remark 5. Thus, letting $V_{\text{sens}}(x_c) = \text{trace}(G_c(t_s, t_f))^2$, and choosing a positive definite function $\phi$, the inequality $\nabla_{x_c} V_{\text{sens}}^T \dot{x}_c \ge \phi(x_c)$ results in an increase of the trace of the CG, as desired.

The energy constraints are encoded by the inequality (11d) obtained starting from the auxiliary I-CBF $h_u$ defined in (9). The slack variable $\delta$ is used to prioritize the objective of keeping the SOC above a minimum threshold over the environmental monitoring task. This way, the latter will be executed as long as the former is satisfied. And, if the energy in the robot battery is depleting, the main active sensing task will be sacrificed just by the amount $\delta$. Moreover, the slack variable $\delta$ makes the feasibility of the NLP (11) hinge on constraint (11d) only, whose satisfaction depends on the function $h_x$, as well as the robot and energy dynamics.

Finally, $\psi$ is a function relating the derivative of the trajectory parameters, $\dot{x}_c$, to the dynamically extended input $v$. In Section V, an example of such a mapping will be given. We conclude this section by stating the main result of the paper which allows for a persistent implementation of environmental monitoring tasks executed through active sensing trajectory optimization.

**Proposition 3.** If a solution to (11) exists for all times, then the dynamically extended control input $v$ allows the robot modeled by (10) to execute persistent environmental monitoring.

*Proof.* Let $v^*$, $x_c^*$, and $\delta^*$ be the solution of (11). Then, the following inequality holds:

$$ \dot{h}_u(x, e, u, v^*) \geq -\gamma_u(h_u(x, e, u)). \quad (12) $$

Theorem 1 in [24] ensures that $h_u(x(t), u(t), e(t)) \ge 0, \forall t \ge 0$. By the definition of $h_u$ in (9), a positive $h_u$ implies that $\dot{h}_x(x, e, u) + \gamma_x(h_x(x, e)) \ge 0$. Then, by Theorem 3 in [4], $h_x(x(t), e(t)) \ge 0, \forall t \ge 0$.

Thus, by (8), $e \ge e_{\min} + \alpha_c (\|p(x) - p_c\|)$, Hence, since $\alpha_c (\|p(x) - p_c\|)$ quantifies the energy required to reach the charging station located at $p_c$, at the charging station $e \ge e_{\min}$. As a result, the SOC never falls below the desired lower threshold $e_{\min}$.

Setting the gradient of the Lagrangian associated with the problem (11) to zero, it immediately follows that $\delta \propto \kappa^{-1}$. Therefore, as $\nabla_{x_c} V_{\text{sens}}^T \dot{x}_c = \dot{V}_{\text{sens}} \ge \phi(x_c) - \delta$, $V_{\text{sens}}$ increases as long as $\phi(x_c) > \delta$, which can be rendered arbitrarily small by increasing the parameter $\kappa$. This condition corresponds to the execution of the environmental monitoring task by maximizing the information gathered along the state trajectory parameterized by $x_c$. $\square$

## V. SIMULATIONS

This section shows an application of persistent monitoring in a planar environment where a gas is spreading from a source, whose location has to be identified. The environmental field is the gas concentration modeled by the following diffusive dynamics [26]:

$$ m: (x, \theta) \mapsto \theta_4 e^{-\frac{(p_1(x) - \theta_1)^2 + (p_2(x) - \theta_2)^2}{\theta_3}} \quad (13) $$

where $[\theta_1 \ \theta_2]^T$ is the source location to be identified, $\theta_3$ and $\theta_4$ determine the spread of the gas concentration, and $[p_1(x), p_2(x)]^T = p(x)$ is the robot position. The dynamics of $\theta$ are as follows:

$$ \dot{\theta} = [0 \ 0 \ K_1 \ -K_2 t^{-\frac{3}{2}}]^T \quad (14) $$

where $K_1$ and $K_2$ have been set to 0.005 and 0.01, respectively. It is worth pointing out that the model (13-14) represents a time-varying gas concentration model in which the unknown parameters $\theta(t) \neq \text{const.}$

The robot executing the persistent environmental monitoring task has been modeled using single integrator dynamics $\dot{x} = u$, $x = [x_1 \ x_2]^T \in \mathbb{R}^2$. In the energy model (6), the function $\alpha_i$ has been chosen to be the identity function, i.e. $\alpha_i(s) = s$. Moreover, following the derivation in [15], the energy required to reach the charging station located at $p_c$ is evaluated as follows:

$$ \alpha_c(\|p(x) - p_c\|) = \log(\|p(x) - p_c\|) - \log(d_{\max}), \quad (15) $$

where $d_{\max}$ is the maximum distance from the charging station at which the robot can recharge its battery. As an example, consider wireless charging technologies: in these settings, $d_{\max}$ represents the footprint of the wireless charging station.

The state trajectory to be optimized has been parameterized as a cubic spline using 4 parameters per each dimension of the state. Then, the parameter vector $x_c$ is given as follows:

$$ x_c = [p_s^{x_1} \ p_f^{x_1} \ t_s^{x_1} \ t_f^{x_1} \ p_s^{x_2} \ p_f^{x_2} \ t_s^{x_2} \ t_f^{x_2}]^T \quad (16) $$

where $p_s^{x_1}, p_f^{x_1}, t_s^{x_1}, t_f^{x_1}$ are the first components of the starting point, ending point, starting tangent, and ending tangent, respectively, of the spline, and $p_s^{x_2}, p_f^{x_2}, t_s^{x_2}, t_f^{x_2}$ are the second components of the same quantities. With the given models, the function $\psi$ in (11) simply becomes the following linear function:

$$ v = \psi(\dot{x}_c) = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \end{bmatrix} \dot{x}_c \quad (17) $$

As a result, (11) is a convex quadratic program in $v, \dot{x}_c$, and $\delta$, and, as such, it can be efficiently solved online. Algorithm 1 summarizes the control strategy employed to obtain the results presented in the following two subsections.

**Remark 4 (Multi-rate estimation and planning).** To improve estimation performance, the state estimation using the EKF in Algorithm 1 can be executed at a higher rate compared to the trajectory optimization implemented by solving (11). In particular, in the simulations shown in this section, the EKF runs at a rate 10 times faster than the trajectory optimization.
<table><caption>**Algorithm 1 Persistent environmental monitoring**</caption><tr><td>1:</td><td>Initialize trajectory parameters x<sub>c</sub></td></tr><tr><td>2:</td><td><b>while</b> true do</td></tr><tr><td>3:</td><td>Execute input v* solution of (11)</td></tr><tr><td>4:</td><td>Collect measurement y</td></tr><tr><td>5:</td><td>Estimate x using EKF on system (4)</td></tr><tr><td>6:</td><td>Update trajectory using x&#775;<sub>c</sub> solution of (11)</td></tr><tr><td>7:</td><td>Re-parameterize trajectory</td></tr><tr><td>8:</td><td><b>end while</b></td></tr></table>

<figure><img src="image_2.png" /><figcaption>**Fig. 1.** Trajectory of the robot executing environmental monitoring obtained by optimizing the state trajectory (solid) and without optimization (dashed).</figcaption></figure>

**Remark 5 (Trajectory re-parameterization).** The parameters $x_c$ of the trajectory are updated at each iteration of the control loop using the optimal $\dot{x}_c$, solution of the optimization problem (11). Thus, the trajectory evolves over time while the robot is tracking it using the input $v = \psi(\dot{x}_c)$. Letting $\Delta t$ be the time horizon used in the definition of $G_c$—i.e., $t_f = t_s + \Delta t$ in (2)— and $dt$ the duration of the control loop, Step 7 in Algorithm 1 ensures that at, iteration $k+1$, the trajectory in the interval $[t_s - dt, t_f - dt]$ is identical to the trajectory in the interval $[t_s, t_f]$ optimized at iteration $k$. This step is justified by the fact that the trace of $G_c$ satisfies the Bellman principle, as observed in [3].

**Remark 6 (Trajectory length and input bounds).** As the trace of $G_c$ defined over the time interval $[t_s, t_f]$ is a measure of the information gathered along the state trajectory from $x(t_s)$ to $x(t_f)$, the optimal $\dot{x}_c$ may lead to an unbounded increase of the length of trajectory, requiring control inputs $v$, and therefore $u$, which exceed the input bounds which exist on a physical robotic platform. Therefore, in practice, enforcing maximum-length constraints on the trajectory may be required. In the case of the cubic spline trajectory parameterization considered in this section, this is simply achieved by upper-bounding the vector of parameters $x_c$.

## A. Environmental monitoring

This section shows the simulation results of the execution of environmental monitoring as an active sensing task implemented using (3), and we compare them with an EKF-based estimation without trajectory optimization. Figure 1 shows the trajectories followed by the robot while optimizing the trace of $G_c$ (solid line), and without this optimization (dashed line). As a result of this trajectory optimization, Fig. 2 reports the graphs of the estimated parameters $\theta$ (Fig. 2a) as well as the trace of $G_c^{-1}$ (Fig. 2b) during the course of the simulation.

<figure><img src="image_3.png" /><figcaption>(a) Dashed lines denote ground truth values.</figcaption></figure>

<figure><img src="image_4.png" /><figcaption>(b) With (solid) and without (dashed) trajectory optimization.</figcaption></figure>

<caption>**Fig. 2.** Environmental monitoring simulation. The estimation of the field parameters $\theta$ and the trace of the inverse of the CG are reported in Figures 2a and 2b, respectively. In Fig. 2b, lower values correspond to more information collected.</caption>

As can be seen, by optimizing the trajectory followed by the robot, the parameters $\theta$ converge to the true ones, depicted as dashed lines in Fig. 2a. Moreover, Fig. 2b clearly shows the advantage brought by the trajectory optimization in terms of the trace of $G_c^{-1}$—lower values indicate more information collected. The solid line is obtained by updating the trajectory as in (3), while the dashed line corresponds to the case without trajectory optimization.

## B. Persistent Environmental monitoring

In this section, energy constraints are included in the scenario considered before. The system dynamics are the same, however, in this case, the trajectory is updated using $\dot{x}_c$ solution of (11) rather than by running the gradient ascent algorithm (3). Moreover, a charging station is located at $p_c = [0, 0]^T$, where the robot is driven to by the input $v$ solution of (11) when in need of energy.

The results are reported in Figures 3 and 4. In particular, the former shows the trajectory followed by the robot (left column) and the energy in the battery of the robot (right column) for each charging cycle (corresponding to each row). Figures 4a and 4b show the estimated parameters $\theta$ and the trace of $G_c^{-1}$. Compared to Figures 2a and 2b, a slight performance detriment can be noticed as a result of the energy constraints that drive the robot away from the optimal trajectory. This corresponds to values of the slack variable $\delta$ which significantly deviate from 0 to allow energy constraints to be satisfied.

# VI. CONCLUSIONS

In this paper, we presented a control-theoretic method for online trajectory planning suitable to be combined with energy control and execute persistent environmental monitoring tasks. The trajectory optimization and the energy constraints are considered holistically in a single optimization problem which, under suitable design choices, becomes a convex quadratic program, and thus can be efficiently solved in an online fashion. We showed the results of multiple simulated scenarios of persistent environmental monitoring applied to the estimation of a gas concentration field that follows diffusive dynamics. In order to illustrate the effectiveness of the proposed approach, we compared it to the case where trajectory optimization or energy constraints are not considered.
<figure><img src="image_5.png" /><figcaption>Fig. 3. Trajectory (left column) and energy (right column) of the robot executing persistent environmental monitoring by optimizing the state trajectory and tracking it using the solution of (11). A charging station located at $[0, 0]^T$ is depicted as a gray circle in the figures on the left column. The first, second, and third discharging-charging cycles are shown in Figures 3a–3b, 3c–3d, and 3e–3f, respectively.</figcaption></figure>

REFERENCES

[1] M. Dunbabin and L. Marques, "Robots for environmental monitoring: Significant advancements and applications," *IEEE Robotics & Automation Magazine*, vol. 19, no. 1, pp. 24–39, 2012.

[2] B. M. Sadler, "Fundamentals of energy-constrained sensor network systems," *IEEE Aerospace and Electronic Systems Magazine*, vol. 20, no. 8, pp. 17–35, 2005.

[3] P. Salaris, M. Cognetti, R. Spica, and P. Robuffo Giordano, "Online optimal perception-aware trajectory generation," *IEEE Trans. Robotics*, vol. 35, no. 6, pp. 1307–1322, 2019.

[4] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, "Control barrier functions: Theory and applications," in *European Control Conference*, 6 2019, pp. 3420–3431.

[5] S. L. Smith, M. Schwager, and D. Rus, "Persistent robotic tasks: Monitoring and sweeping in changing environments," *IEEE Trans. Robotics*, vol. 28, no. 2, pp. 410–426, 2011.

[6] A. Dhariwal, G. S. Sukhatme, and A. A. Requicha, "Bacterium-inspired robots for environmental monitoring," in *Proc. IEEE International Conf. Robotics and Automation*, vol. 2, 2004, pp. 1436–1443.

[7] M. Trincavelli, M. Reggente, S. Coradeschi, A. Loutfi, H. Ishida, and A. J. Lilienthal, "Towards environmental monitoring with mobile robots," in *IEEE/RSJ International Conf. Intelligent Robots and Systems*, 2008, pp. 2210–2215.

[8] R. Khodayi-mehr, W. Aquino, and M. M. Zavlanos, "Model-based active source identification in complex environments," *IEEE Trans. Robotics*, vol. 35, no. 3, pp. 633–652, 2019.

[9] L. Mihaylova, T. Lefebvre, H. Bruyninckx, K. Gadeyne, and J. De Schutter, "A comparison of decision making criteria and optimization methods for active robotic sensing," in *International Conf. Numerical Methods and Applications*, 2002, pp. 316–324.

<figure><img src="image_6.png" /><figcaption>Fig. 4. Persistent environmental monitoring. The estimation of the field parameters $\theta$ and the trace of the inverse of the CG are shown in Figures 4a and 4b, respectively. In Fig. 4a, ground truth values are depicted as dashed lines. For reference, in Fig. 4b, the performance of the system without energy constraints—shown in Fig. 2b as a solid line—is depicted as a dashed line.</figcaption></figure>

[10] T. H. Chung, V. Gupta, J. W. Burdick, and R. M. Murray, "On a decentralized active sensing strategy using mobile sensor platforms in a network," in *Proc. IEEE Conf. Decision and Control (CDC)*, vol. 2, 2004, pp. 1914–1919.

[11] J. Le Ny and G. J. Pappas, "On trajectory optimization for active sensing in gaussian process models," in *Proc. IEEE Conf. Decision and Control (CDC)*, 2009, pp. 6286–6292.

[12] R. Ouyang, K. H. Low, J. Chen, and P. Jaillet, "Multi-robot active sensing of non-stationary gaussian process-based environmental phenomena," in *Proc. International Conf. Autonomous Agents and Multiagent System*, 2014.

[13] X. He, J. R. Bourne, J. A. Steiner, C. Mortensen, K. C. Hoffman, C. J. Dudley, B. Rogers, D. M. Cropek, and K. K. Leang, "Autonomous chemical-sensing aerial robot for urban/suburban environmental monitoring," *IEEE Systems Journal*, vol. 13, no. 3, pp. 3524–3535, 2019.

[14] S. Martin and P. Corke, "Long-term exploration & tours for energy constrained robots with online proprioceptive traversability estimation," in *Proc. IEEE International Conf. Robotics and Automation (ICRA)*, 2014, pp. 5778–5785.

[15] G. Notomista, S. F. Ruf, and M. Egerstedt, "Persistification of robotic tasks using control barrier functions," *Robotics and Automation Letters*, vol. 3, no. 2, pp. 758–763, 2018.

[16] G. Notomista and M. Egerstedt, "Persistification of robotic tasks," *IEEE Transactions on Control Systems Technology*, vol. 29, no. 2, pp. 756–767, 2020.

[17] S. Piller, M. Perrin, and A. Jossen, "Methods for state-of-charge determination and their applications," *Journal of Power Sources*, vol. 96, no. 1, pp. 113–120, 2001.

[18] U. Krewer, F. Röder, E. Harinath, R. D. Braatz, B. Bedürftig, and R. Findeisen, "Dynamic models of Li-Ion batteries for diagnosis and operation: A review and perspective," *Journal of The Electrochemical Society*, vol. 165, no. 16, p. A3656, 2018.

[19] W.-Y. Chang, "The state of charge estimating methods for battery: A review," *International Scholarly Research Notices*, vol. 2013, 2013.

[20] M. Murnane and A. Ghazel, "A closer look at state of charge (SOC) and state of health (SOH) estimation techniques for batteries," *Analog Devices*, vol. 2, pp. 426–436, 2017.

[21] L. Zhao, M. Lin, and Y. Chen, "Least-squares based Coulomb counting method and its application for state-of-charge (SOC) estimation in electric vehicles," *International Journal of Energy Research*, vol. 40, no. 10, pp. 1389–1399, 2016.

[22] H. A. Toliyat and G. B. Kliman, *Handbook of electric motors*. CRC press, 2018, vol. 120.

[23] Y. Huang, S. Z. Yong, and Y. Chen, "Guaranteed vehicle safety control using control-dependent barrier functions," in *American Control Conference*, 2019, pp. 983–988.

[24] A. D. Ames, G. Notomista, Y. Wardi, and M. Egerstedt, "Integral control barrier functions for dynamically defined control laws," *IEEE Control Systems Letters*, vol. 5, no. 3, pp. 887–892, 2020.

[25] G. Notomista, "Long-duration robot autonomy: From control algorithms to robot design," Ph.D. dissertation, Georgia Institute of Technology, 2020.

[26] J. Crank, *The mathematics of diffusion*. Oxford university press, 1979.