# A Semi-Lagrangian Approach for Time and Energy Path Planning Optimization in
Static Flow Fields

Víctor C. da S. Campos, Armando A. Neto, and Douglas G. Macharet This work has
been financed by the CAPES - Finance Code 001, CNPq - grant numbers
310446/2021-0, 306286/2020-3 and 407063/2021-8, and FAPEMIG - grant number
APQ-02228-22.V. C. S. Campos and A. A. Neto are with the Dept. of Electronics
Engineering, Univ. Federal de Minas Gerais, Belo Horizonte, Brazil.D. G.
Macharet is with the Computer Vision and Robotics Laboratory (VeRLab), Dep. of
Computer Science, Univ. Federal de Minas Gerais, Belo Horizonte, Brazil.

###### Abstract

Efficient path planning for autonomous mobile robots is a critical problem
across numerous domains, where optimizing both time and energy consumption is
paramount. This paper introduces a novel methodology that considers the
dynamic influence of an environmental flow field and considers geometric
constraints, including obstacles and forbidden zones, enriching the complexity
of the planning problem. We formulate it as a multi-objective optimal control
problem, propose a novel transformation called Harmonic Transformation, and
apply a semi-Lagrangian scheme to solve it. The set of Pareto efficient
solutions is obtained considering two distinct approaches: a deterministic
method and an evolutionary-based one, both of which are designed to make use
of the proposed Harmonic Transformation. Through an extensive analysis of
these approaches, we demonstrate their efficacy in finding optimized paths.

###### Index Terms:

Motion and Path Planning, Collision Avoidance, Optimization and Optimal
Control, Multi-objective Optimal Control

##  I Introduction

The continual research and development of new and more advanced path planning
approaches play a pivotal role in Robotics [1]. Such techniques enable
autonomous mobile robots to navigate efficiently and safely in complex and
dynamic environments, making them essential for various applications, from
logistics to monitoring and exploration. In this context, new challenges arise
when robotic systems address not just a singular objective but multiple and
often conflicting goals. These objectives can range from minimizing travel
time and energy consumption simultaneously to optimizing factors like safety
and resource allocation [2].

In single-objective approaches, the most commonly prioritized factors are
typically the path’s length [3, 4] and travel time [5, 6]. However, by
incorporating other additional attributes, such as path safety/vulnerability
and smoothness [7, 8], we can significantly improve both the quality and the
applicability of results. Regarding the more general class of routing
problems, where a sequence of visits is demanded, a multi-objective variant of
the Orienteering Problem (OP) was proposed in [9], where the goal was to
maximize the cumulative reward obtained while concurrently minimizing the
exposure to sensors deployed in the environment.

Furthermore, it is also imperative to acknowledge that, in numerous domains,
environmental dynamics substantially influence the trajectories and behaviors
of the vehicles. This is particularly evident in fields such as aerospace,
where factors like air density, wind patterns, and gravitational forces
intricately shape the aircraft flight paths [10]. Similarly, in maritime
environments, the varying properties of water, including currents and
turbulence, substantially impact the maneuverability of underwater or surface
vehicles [11].

Therefore, an adequate navigation strategy holds the potential to generate
paths that optimize robot movement according to the surrounding flow field
resulting from atmospheric and/or ocean currents. This synergy between path
planning and environmental dynamics enhances the efficiency and speed of
vehicle navigation and bolsters adaptability, ensuring that robots can
navigate seamlessly through environments where flow dynamics are significant.
Ultimately, this approach fosters improved resource utilization and reduced
energy consumption, increasing the system’s performance across a broad
spectrum of robotic applications.

The literature offers a variety of approaches to tackling environmental flow
dynamics. These methods include graph-based methods [12] and evolutionary
algorithms [13], which prioritize energy-efficient path planning, as well as
sampling-based planners [14], which focus on achieving time-optimal paths. It
is important to highlight that in scenarios with constant thrust (velocity of
the vehicle concerning the flow), minimizing energy is equivalent to
minimizing time. In contrast, in situations considering a constant net speed,
the travel time is directly proportional to the path length.

In this context, multi-objective formulations have emerged as a noteworthy
approach. For instance, [15] introduces a particle swarm optimization-based
algorithm that considers multiple objectives, including travel length, path
smoothness, economic cost, and path safety. Methods that integrate both task
and path planning problems are presented in [16, 17], whose goals are to
maximize the collected reward while minimizing the energy expenditure,
considering single and multi-robot systems. In another recent study, [18]
employs a level set method to ascertain energy-time optimal solutions within
dynamic flow environments.

In this paper, we introduce an innovative approach to deal with time and
energy-efficient path planning within environments characterized by static
flow fields and stationary obstacles. This problem is posed as a multi-
objective optimal control problem with forbidden zones in the state space. We
highlight the following contributions:

  * •

A _novel_ transformation, dubbed _Harmonic Transformation_ , is employed to
map values onto the $[0,1]$ range to deal with the forbidden zones and avoid
possible numerical problems in the computation of the value functions. We show
that, given that the corresponding assumptions hold, a semi-Lagrangian
approach converges to the unique viscosity solution of the corresponding
transformed partial differential equations;

  * •

Considering that time and energy are usually conflicting costs, we propose two
approaches to find the set of _Pareto efficient solutions_ in a multi-
objective manner: a _deterministic_ one, based on solving multiple single-
objective optimizations concurrently; and an _evolutionary_ approach, based on
a proper multi-objective evolutionary algorithm.

The remainder of this paper is structured as follows: Section II delineates
the problem formulation and introduces the Harmonic Transformation,
demonstrating its application within a dynamic programming framework; Section
III elaborates on the two distinct approaches we propose to address the multi-
objective problem; Section IV presents numerical results, showcasing the
efficacy of our proposed approaches; Lastly, in Section V, we provide
concluding remarks and outline potential avenues for future research.

##  II Problem formulation

Given a compact region of interest in the state space, we consider an agent
described by:

$\displaystyle\dot{\mathbf{x}}$
$\displaystyle=\mathbf{f}(\mathbf{x},\mathbf{u}),$ (1)
$\displaystyle=\mathbf{f}_{1}(\mathbf{x})+F_{2}(\mathbf{x})\mathbf{u},$ (2)

with $\mathbf{x}\in\mathds{{R}}^{n}$ being the agent’s states, and
$\mathbf{u}\in\mathcal{U}$ the control inputs in the allowable control inputs
set $\mathcal{U}\subset\mathds{{R}}^{m}$. Also,
$\mathbf{f}(\mathbf{x},\mathbf{u}):\mathds{{R}}^{n+m}\rightarrow\mathds{{R}}^{n}$
represents the agent’s dynamics, which can be decomposed into
$\mathbf{f}_{1}(\mathbf{x}):\mathds{{R}}^{n}\rightarrow\mathds{{R}}^{n}$ (the
_flow_ vector field) and
$\mathbf{F}_{2}(\mathbf{x}):\mathds{{R}}^{n}\rightarrow\mathds{{R}}^{n\times
m}$ (the _steering_ matrix).

In the context of optimal control, we consider a cost function,
$\ell(\mathbf{x},\mathbf{u}):\mathds{{R}}^{n+m}\rightarrow\mathds{{R}}$, that
attributes a cost to every pair $(\mathbf{x},\mathbf{u})$ whose $\mathbf{x}$
is not a _target state_. We can also define a _value function_
$v(\mathbf{x}):\mathds{{R}}^{n}\rightarrow\mathds{{R}}$ describing the minimum
value for each point, $\mathbf{x}$, in state space (with a trajectory starting
at this point), such that:

$\displaystyle
v(\mathbf{x})=\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{\infty}\ell(\mathbf{x},\mathbf{u})dt.$
(3)

Since any path starting from a point along the optimal path should be optimal,
we can use a Dynamic Programming Principle for $v(\mathbf{x})$ of the form:

$\displaystyle
v(\mathbf{x})=\inf_{\mathbf{u}\in\mathcal{U}}\left\\{v(\mathbf{y}_{x}(\Delta
t,\mathbf{u}))+\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}),\mathbf{u})dt\right\\},$ (4)

with $\mathbf{y}_{x}(t,\mathbf{u})$ representing the point at time $t$ along
the path, taken when considering the control input defined by $\mathbf{u}(t)$,
for the system dynamics in (1).

To deal with constraints on $\mathbf{x}$, such as forbidden or dangerous
zones, we consider that the value function must be infinite in these
locations. In addition, assuming that $\ell(\mathbf{x},\mathbf{u})$ is always
non-negative, the value of the target location must always be null. When
considered together, these constraints lead to the boundary conditions:

$\displaystyle v(\mathbf{x})=\begin{cases}0&\textrm{for
}\mathbf{x}(t)=\mathbf{x}_{g},\\\ \infty&\textrm{for
}\mathbf{x}(t)\in\partial\mathcal{O},\end{cases}$ (5)

with $\mathbf{x}_{g}$ representing a desired target location, and
$\partial\mathcal{O}$ representing the boundaries of forbidden regions
$\mathcal{O}$.

###  II-A Harmonic Transformation and Dynamic Programming

To properly deal with obstacles/forbidden zones, we use a transformation so
that the value function image is mapped from $[0,\infty)$ to $[0,1)$. Although
the _Kruzkov Transformation_ is usually employed in these cases, it can lead
to numerical problems when the value function assumes large values [19]. This
problem can be somewhat mitigated by employing a scalar multiplying
$v(\mathbf{x})$, but finding a suitable scalar can be a tiresome process in
some cases. In this sense, we consider a transformation of the form:

$\bar{v}(\mathbf{x})=\mathcal{H}(v(\mathbf{x}))=\dfrac{v(\mathbf{x})}{1+v(\mathbf{x})}=1-\dfrac{1}{1+v(\mathbf{x})},$

hereinafter referred to as _Harmonic Transformation_ , $\mathcal{H}(.)$.

As we shall detail subsequently, to derive a suitable Dynamic Programming
Principle for the transformed problem, the following property of this
transformation is essential:

$\displaystyle\mathcal{H}(x_{1}+x_{2})$
$\displaystyle=\dfrac{x_{1}+x_{2}}{1+x_{1}+x_{2}}\dfrac{\frac{1}{1+x_{1}}}{\frac{1}{1+x_{1}}},$
$\displaystyle=\dfrac{\mathcal{H}(x_{1})+\frac{x_{2}}{1+x_{1}}+x_{2}-x_{2}}{\mathcal{H}(x_{1})+\frac{1+x_{2}}{1+x_{1}}+(1+x_{2})-(1+x_{2})},$
$\displaystyle=\dfrac{\mathcal{H}(x_{1})+x_{2}-x_{2}\left(1-\frac{1}{1+x_{1}}\right)}{1+\mathcal{H}(x_{1})+x_{2}-(1+x_{2})\left(1-\frac{1}{1+x_{1}}\right)},$
$\displaystyle=\dfrac{\mathcal{H}(x_{1})+\left(1-\mathcal{H}(x_{1})\right)x_{2}}{1+\mathcal{H}(x_{1})+x_{2}-(1+x_{2})\mathcal{H}(x_{1})},$
$\displaystyle\mathcal{H}(x_{1}+x_{2})$
$\displaystyle=\dfrac{\mathcal{H}(x_{1})+\left(1-\mathcal{H}(x_{1})\right)x_{2}}{1+\left(1-\mathcal{H}(x_{1})\right)x_{2}}.$
(6)

####  II-A1 Numerical approximation

By applying property (6), a Dynamic Programming Principle can be found for the
transformed value function:

$\displaystyle\bar{v}(\mathbf{x}(t))$
$\displaystyle=\inf_{\mathbf{u}\in\mathcal{U}}\left\\{\dfrac{\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}))+q(\mathbf{x}(t),\mathbf{u})}{1+q(\mathbf{x}(t),\mathbf{u})}\right\\},$
(7) $\displaystyle q(\mathbf{x}(t),\mathbf{u})$
$\displaystyle=\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}),\mathbf{u})dt,$

and a semi-Lagrangian (SL) numerical scheme can be used to approximate the
solution by employing a time discretization, followed by a space
discretization.

We consider the time discretization of Eq. (7), with step $\Delta t$, by
applying a trapezoidal approximation for the integral term

$\displaystyle\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}),\mathbf{u})dt$
$\displaystyle\approx\dfrac{\Delta
t}{2}\left(\ell(\mathbf{x}_{k+1},\mathbf{u}_{k})+\ell(\mathbf{x}_{k},\mathbf{u}_{k})\right),$
$\displaystyle g(\mathbf{x}_{k},\mathbf{u}_{k})$ $\displaystyle=\dfrac{\Delta
t}{2}\left(\ell(\mathbf{x}_{k+1},\mathbf{u}_{k})+\ell(\mathbf{x}_{k},\mathbf{u}_{k})\right),$
(8)

and a trapezoidal method to solve the system of equations composed of (1)
(assuming that the control input is held constant between the two samples),
leading to

$\displaystyle\mathbf{x}_{k+1}=\mathbf{x}_{k}+\dfrac{\Delta
t}{2}\left(\mathbf{f}(\mathbf{x}_{k},\mathbf{u}_{k})+\mathbf{f}(\mathbf{x}_{k+1},\mathbf{u}_{k})\right),$
(9) $\displaystyle\bar{v}_{k}(\mathbf{x}_{k})$
$\displaystyle\quad=\inf_{\mathbf{u}\in\mathcal{U}}\left\\{\dfrac{\bar{v}_{k+1}(\mathbf{x}_{k+1})\\!+\\!\left(1\\!-\\!\bar{v}_{k+1}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}{1+\left(1-\bar{v}_{k+1}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}\right\\}.$
(10)

Next, we perform the space discretization of $\bar{v}$ by considering an
unstructured grid of points covering the state space. Once these points are
used to represent $\bar{v}$, they are the only points over the space for which
the value is updated. And since $\bar{v}(\mathbf{x}_{k+1})$ might not be a
part of the grid, it is replaced by a finite element _linear_ interpolation
over the grid. One way of doing this linear interpolation is by employing a
Delaunay triangulation on the unstructured grid points to find a triangulation
of the space. We consider these triangles as our finite elements and represent
the interpolation of $\bar{v}(\mathbf{x}_{k+1})$ as
$\mathcal{I}_{\overline{v}_{k+1}}[\mathbf{x}_{k+1}]$.

Taken together, both discretizations (time and space) lead to an SL
approximation scheme of (7), in the form:

$\bar{v}_{k}(\mathbf{x}_{k})=\inf_{\mathbf{u}\in\mathcal{U}}\left\\{\dfrac{\mathcal{I}_{\bar{v}_{k+1}}[\mathbf{x}_{k+1}]+\left(1-\mathcal{I}_{\bar{v}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}{1+\left(1-\mathcal{I}_{\bar{v}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}\right\\},$

(11)

with boundary conditions

$\bar{v}_{k}(\mathbf{x}_{k})=\begin{cases}0&\textrm{if
}\mathbf{x}(t)=\mathbf{x}_{g},\\\ 1&\textrm{if
}\mathbf{x}(t)\in\partial\mathcal{O}.\end{cases}$ (12)

As in most approaches based on Dynamic Programming (DP), Eq. (11) can be
solved backward in time using a _value iteration_ algorithm. Since we have
formulated our problem as a stationary/infinite horizon optimal control
problem, an acceleration technique known as _policy iteration_ [19, Section
8.4.7] can also be employed.

At every grid point, the optimal policy is:

$\displaystyle\mathbf{u}_{k}\\!=\\!\underset{\mathbf{u}_{k}\in\mathcal{U}}{\operatorname{argmin}}\\!\left\\{\dfrac{\mathcal{I}_{\bar{v}}[\mathbf{x}_{k+1}]+\left(1\\!-\\!\mathcal{I}_{\bar{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}{1+\left(1-\mathcal{I}_{\bar{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}\right\\},$
(13)

and fixed for this iteration. Afterward, the value function is updated
according to conditions (12) and

$\displaystyle\bar{v}(\mathbf{x}_{k})\left(1+\left(1-\mathcal{I}_{\bar{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})\right)$
$\displaystyle\quad\quad\quad-(1-g(\mathbf{x}_{k},\mathbf{u}_{k}))\mathcal{I}_{\bar{v}}[\mathbf{x}_{k+1}]=g(\mathbf{x}_{k},\mathbf{u}_{k}).$
(14)

The algorithm iterates until it converges to the minimum of the value
function.

Finally, the original value function (3) can be recovered by:

$v(\mathbf{x}_{k})=\dfrac{\bar{v}(\mathbf{x}_{k})}{1-\bar{v}(\mathbf{x}_{k})}.$

One of the main problems in this case is that Eq. (14) defines a set of
nonlinear equations. However, it is important to note that this is a highly
sparse problem, for which specialized solvers drastically increase
performance.

The DP Principle in Eq. (7) can be recast as a partial differential equation,
known as the Hamilton-Jacobi-Bellman (HJB) equation for the transformed value
function of the form:

$\displaystyle\sup_{\mathbf{u}\in\mathcal{U}}\left\\{-\nabla\bar{v}(\mathbf{x})\cdot\mathbf{f}(\mathbf{x},\mathbf{u})-\left(1-\bar{v}(\mathbf{x})\right)^{2}\ell(\mathbf{x},\mathbf{u})\right\\}=0.$
(15)

With this representation in mind, considering the usual regularity conditions
in the literature to ensure that the value function is continuous, we can
state the following result regarding the optimal control problem and the
proposed SL approximation scheme.

######  Theorem 1.

Consider the optimal control problem represented by the HJB equation (15). As
long as $\mathbf{f}$ and $\ell$ are Lipschitz, and $\ell$ is positive
definite, with regards to the states, there exists a unique viscosity solution
to this equation, representing the transformed value function of the optimal
control problem. In addition to this, if $\ell>0$, for every $\mathbf{x}$
different from the target state, $\ell$ is convex with regards to
$\mathbf{u}$, $\mathbf{u}$ is bounded, and $\mathbf{f}$ is bounded, the
proposed numerical scheme converges to this unique solution as the time step,
$\Delta t$, and the maximum distance between points on the grid, $\Delta x$,
tend to zero, so long as $\Delta x$ tends faster than $\Delta t$.

###### Proof.

The proof of this theorem is presented in Appx. -A. ∎

###  II-B Energy and time value functions

Even though we have shown that an SL scheme can be employed with the _Harmonic
Transformation_ with any cost function $\ell$ that satisfies the conditions of
Thm. 1, in this work, we will focus our attention on two specific costs, and
the multi-objective problem defined by them.

Assuming that

$\ell_{T}(\mathbf{x},\mathbf{u})=\begin{cases}1&\textrm{if
}\mathbf{x}\neq\mathbf{x}_{g},\\\ 0&\textrm{if }\mathbf{x}=\mathbf{x}_{g},\\\
\end{cases}$ (16)

its value function in Eq. (3) can be written as:

$v_{T}(\mathbf{x})=\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{\infty}\\!\ell_{T}(\mathbf{x},\mathbf{u})dt=\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{T(\mathbf{x})}\\!1dt=T(\mathbf{x}),$
(17)

which corresponds to the _time taken to reach the target state from the
current state_ , hereinafter referred to as _time value function_.

We aim to examine the cost incurred in the following form:

$\ell(\mathbf{x},\mathbf{u})=\left\\{\begin{array}[]{ll}\mathbf{u}^{T}\mathbf{u},&\textrm{if
}\mathbf{x}\neq\mathbf{x}_{g},\\\ 0,&\textrm{if }\mathbf{x}=\mathbf{x}_{g},\\\
\end{array}\right.$

so that its value function represents the _energy required to reach the target
state from the current state_ (assuming the energy is given by the squared
$\mathcal{L}_{2}$ norm of the control inputs), it could lead to an _ill-posed_
optimal control problem since the agent could decide to stop _indefinitely_ at
any _equilibrium point_ of its dynamics, without being penalized.

In that regard, we consider the cost:

$\ell_{E}(\mathbf{x},\mathbf{u})=\begin{cases}\varepsilon+\mathbf{u}^{T}\mathbf{u}&\textrm{if
}\mathbf{x}\neq\mathbf{x}_{g},\\\ 0&\textrm{if }\mathbf{x}=\mathbf{x}_{g},\\\
\end{cases}$ (18)

with $\varepsilon$ being a small scalar that penalizes the time taken to reach
the target state. Consequently, its value function in Eq. (3) can be written
as:

$\displaystyle v_{E}(\mathbf{x})$
$\displaystyle=\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{\infty}\\!\\!\\!\\!\ell_{E}(\mathbf{x},\mathbf{u})dt=\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{T(\mathbf{x})}\\!\\!(\varepsilon+\mathbf{u}^{T}\mathbf{u})dt,$
$\displaystyle=\varepsilon
T(\mathbf{x})+\inf_{\mathbf{u}\in\mathcal{U}}\int_{0}^{T(\mathbf{x})}\\!\\!\mathbf{u}^{T}\mathbf{u}\;dt,$
(19)

and can be interpreted as _the energy used to reach the target state plus a
small penalization of the time taken to reach it from the current state_. We
will refer to this value function as the _energy value function_.

Since these two cost functions (and respective value functions) are
antagonistic, in this work we try to find a set of _Pareto efficient
solutions_ (described either by the corresponding value function or policy
defined for the agent), in a multi-objective manner. In that regard, we
present two approaches to this multi-objective optimization: a _deterministic_
one (based on solving multiple single-objective optimizations concurrently),
and an _evolutionary_ one (based on a proper multi-objective evolutionary
algorithm).

##  III Proposed approaches

###  III-A Concurrent Policy Iteration

A simple and direct way of dealing with this multi-objective optimal control
problem would be to perform a scalarization of the cost function:

$\ell_{\alpha}(\mathbf{x},\mathbf{u})=\alpha\ell_{T}(\mathbf{x},\mathbf{u})+(1\\!-\\!\alpha)\ell_{E}(\mathbf{x},\mathbf{u}),~{}~{}\alpha\in[0,1],$
(20)

which leads to the suitable value function in Eq. (3). A grid could be
performed on this _convex scalar weight_ $\alpha$, which would lead to
different mono-objective problems that could be solved separately and
correspond to different efficient solutions on the Pareto set of optimal
solutions for this problem.

One way to improve this proposal is by noting that, despite solving for
different value functions, all these optimal control problems explore the same
system (described by the agent’s dynamics, target states, and
obstacles/forbidden regions). In that regard, the policies could be evaluated
under all of the single-objective costs/value functions simultaneously leading
to a higher _exploration_ of the optimal control problem.

In this setting, a _naive_ implementation, considering policy iteration and a
grid of $n_{p}$ scalar points in $\alpha$, would consist of updating a policy
for each grid point, but calculating all $n_{p}$ value functions for each of
these points. The main advantage of this approach is that it allows, in a
sense, the different policies to share information by means of the many value
functions considered. The main drawback is that it requires the update of
$n_{p}^{2}$ value functions at each time step, and this is the most costly
step in many cases.

If we were not dealing with transformed value functions, it would suffice to
consider the time and energy value functions in (17) and (19), respectively,
since the rest of the value functions could be composed by taking the convex
combination of them using $\alpha$. Since we are dealing with value functions
transformed by the proposed _Harmonic Transformation_ , we need to define a
relationship between them, and the transformation of a convex combination of
these two value functions. In that regard, we can write

$\displaystyle\mathcal{H}(\alpha v_{T}+(1\\!-\\!\alpha)v_{E})=\dfrac{\alpha
v_{T}+(1\\!-\\!\alpha)v_{E}}{1+\alpha v_{T}+(1\\!-\\!\alpha)v_{E}}$
$\displaystyle\quad=\dfrac{\alpha\frac{\mathcal{H}(v_{T})}{1-\mathcal{H}(v_{T})}+(1\\!-\\!\alpha)\frac{\mathcal{H}(v_{E})}{1-\mathcal{H}(v_{E})}}{1+\alpha\frac{\mathcal{H}(v_{T})}{1-\mathcal{H}(v_{T})}+(1\\!-\\!\alpha)\frac{\mathcal{H}(v_{E})}{1-\mathcal{H}(v_{E})}}$
$\displaystyle\quad=\dfrac{\alpha\mathcal{H}(v_{T})+(1\\!-\\!\alpha)\mathcal{H}(v_{E})-\mathcal{H}(v_{T})\mathcal{H}(v_{E})}{1-(1\\!-\\!\alpha)\mathcal{H}(v_{T})-\alpha\mathcal{H}(v_{E})}.$
(21)

By applying Eq. (21), we can find any of the transformed value functions (for
a given policy) as long as we have the transformed time and energy value
functions. So, only two value functions are updated and stored for every
policy, and only $2n_{p}$ value functions are updated at each step, leading to
the Concurrent Policy Iteration procedure presented in Alg. 1.

Algorithm 1 Concurrent Policy Iteration (CPI)

0: $\mathbf{x}_{g}$, $\bm{\alpha}$, $\varepsilon$

1: $k\leftarrow 0$

2: for each $\alpha_{i}\in\bm{\alpha}$ do

3: Assign a corresponding initial policy $\mathbf{u}_{0}^{(i)}$

4: Calculate the transformed time value $\bar{v}_{T0}^{(i)}$ of using the
$\mathbf{u}_{0}^{(i)}$ policy, the cost in (16), and (9), (8) and (14)

5: Calculate the transformed energy value $\bar{v}_{E0}^{(i)}$ of using the
$\mathbf{u}_{0}^{(i)}$ policy, the cost in (18), and (9), (8) and (14)

6: while policies $\mathbf{u}_{k}^{(i)}$ have not converged do

7: $k\leftarrow k+1$

8: for each $\alpha_{i}\in\bm{\alpha}$ do

9: Find the _best_ current value for $\alpha_{i}$ by
$\displaystyle\tilde{v}^{(i)}(\mathbf{x})=$
$\displaystyle\quad\min_{s}\dfrac{\alpha_{i}\bar{v}_{T(k-1)}^{(s)}+(1\\!-\\!\alpha_{i})\bar{v}_{E(k-1)}^{(s)}-\bar{v}_{T(k-1)}^{(s)}\bar{v}_{E(k-1)}^{(s)}}{1-(1\\!-\\!\alpha_{i})\bar{v}_{T(k-1)}^{(s)}-\alpha_{i}\bar{v}_{E(k-1)}^{(s)}}$

10: Updates the policy by using the cost (20) defined by $\alpha_{i}$ using
(9), (8) and
$\mathbf{u}_{k}^{(i)}=\underset{\mathbf{u}_{k}\in\mathcal{U}}{\operatorname{argmin}}\left\\{\dfrac{\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]+\left(1-\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}{1+\left(1-\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}\right\\}$

11: Calculate the transformed time value $\bar{v}_{Tk}^{(i)}$ using the
$\mathbf{u}_{k}^{(i)}$ policy, cost in (16), and (9), (8) and (14)

12: Calculate the transformed energy value $\bar{v}_{Ek}^{(i)}$ using the
$\mathbf{u}_{k}^{(i)}$ policy, cost in (18), and (9), (8) and (14)

13: return Policies $\mathbf{u}_{k}^{(i)}$, and value functions
$\bar{v}_{Tk}^{(i)}$ and $\bar{v}_{Ek}^{(i)}$

###  III-B Genetic Algorithm and Evolutionary Policy Iteration

While the proposed CPI algorithm exchanges information about the single-
objective problems being solved to obtain better and faster results, it still
consists, essentially, of solving $n_{p}$ single-objective optimal control
problems. In addition to this, to find the optimal policies, it is common to
consider a finite set of allowable control inputs (so that the minimum can be
found from a simple verification of all possible elements).

To overcome both problems, here we propose a multi-objective genetic
algorithm. To deal with the problem in a multi-objective approach directly, we
employ the NSGA-II [20] (specifically its _fast non-dominated sorting_ and
_crowding distance_ procedures), whereas the Evolutionary Policy Iteration
[21, 22] approach is adopted to deal with a large/continuous set of allowable
controls.

In this paper, we use policies $\mathbf{u}$ to represent the individuals in
the population. Equation (14) is employed with costs (16) and (18) to
associate time and energy value functions to these policies, respectively.
Note that the optimal policy, when considering any value function, would lead
to an overall smaller value over the grid of unstructured points (since it
needs to be the best policy everywhere inside of our region of interest). In
that regard, we use the average of the value function over the unstructured
grid as a proxy for how good that policy is, and end up with a bi-objective
optimization problem with average time value and average energy value as our
costs. Note that, since we are employing an _Harmonic Transformation_ on both
value functions, they assume values in $[0,1]$ and, as such, so will their
average values.

In a single-objective setting, _Evolutionary Policy Iteration_ [21] makes use
of the concept of _Policy Switching_ to generate an elite individual which is
guaranteed to be an improvement over the policies in the current population.
In our setting, _Policy Switching_ can be written, with regards to a specific
value function, as

$\displaystyle\tilde{v}(\mathbf{x}_{k})\\!$
$\displaystyle=\\!\\!\min_{i\in\textrm{population}}\\!\\!v_{i}(\mathbf{x}_{k}),$
$\displaystyle\mathbf{u}_{ps}(\mathbf{x}_{k})$
$\displaystyle=\\!\\!\underset{\mathbf{u}_{k}\in\mathcal{U}_{pop}(\mathbf{x}_{k})}{\operatorname{argmin}}\\!\\!\left\\{\\!\dfrac{\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]\\!+\\!\left(1\\!-\\!\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}{1\\!+\\!\left(1\\!-\\!\mathcal{I}_{\tilde{v}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u}_{k})}\\!\right\\},$

with $\tilde{v}(\mathbf{x}_{k})$ representing the value function obtained by
taking the minimum value over the individuals of the population,
$\mathcal{U}_{pop}(\mathbf{x}_{k})$ the set of controls defined by the
policies in the population at point $\mathbf{x}_{k}$, and
$\mathbf{u}_{ps}(\mathbf{x}_{k})$ the policy defined by taking the best
control available in $\mathcal{U}_{pop}(\mathbf{x}_{k})$ according to
$\tilde{v}(\mathbf{x}_{k})$. _Policy Switching_ can also be employed as a
_cross-over_ operator that can generate an offspring given a set of parents,
in which case a subset of the population is usually employed as the parents.
In a single-objective setting, it can be shown that when paired with a
_mutation_ operator to ensure the exploration of the policy space, _policy
switching_ ensures the elite policy convergence to the optimal policy with
probability one, when the state space is finite [22].

In our multi-objective setting, we can still employ _policy switching_ to
generate elite individuals. Since every individual already possesses time and
energy value functions associated with its policy, it is straightforward to
generate elite individuals concerning time and energy. In addition to this,
considering a scalar parameter $\alpha\in[0,1]$, we can employ (21) to recover
elite individuals which are not on the extreme points of the Pareto set. In
that regard, in our proposed algorithm, we generate three elite individuals
using _policy switching_ at every generation, the two extreme policies
(associated with minimum time and minimum energy), and a third one using a
random $\alpha$ at every generation. Even though the value of $\alpha$ could
be fixed, at $\alpha=0.5$ for instance, we employ a random $\alpha$ to
increase exploration of the Pareto Set. To increase the convergence of the
method, as well as ensure some diversity in the population, this elite
individual with a random $\alpha$ also considers some control possibilities
from a fixed set of control actions (aside from the ones on the individuals on
the population) when searching for the value of the best action.

Similarly, _policy switching_ is also employed as a _cross-over_ operator,
with a random $\alpha\in[0,1]$, to generate other new offspring in the
population. In this case, however, only a small number of individuals (usually
2 or 3) are employed as parents. To create new individuals, part of the
offspring is also created by employing a simple _cross-over_ between two
policies. A random scalar value $\lambda\in[0,1]$ is chosen and given policies
$\mathbf{u}_{1}(\mathbf{x}_{k})$ and $\mathbf{u}_{2}(\mathbf{x}_{k})$, a new
policy is generated by

$\displaystyle\mathbf{u}_{\textrm{off}}=\lambda\mathbf{u}_{1}(\mathbf{x}_{k})+(1-\lambda)\mathbf{u}_{2}(\mathbf{x}_{k}).$
(22)

With both _cross-over_ operators, a simple _mutation_ with a Gaussian noise
(but modified to respect the bounds of the allowable control set) is employed
in these new individuals to ensure exploration of the policy space.

As in a standard NSGA-II algorithm, the _fast non-dominated sorting_ procedure
is employed to rank solutions according to dominance in the objective
functions space, whereas the _crowding distance_ is employed to rank solutions
on the same level (which do not dominate one another). These ranking solutions
are employed when selecting individuals from the population (both for choosing
parents for the _cross-over_ operators and for selecting survivors for the
next generation). Parents are chosen according to a binary tournament, whereas
survivors (from the combined original and offspring population) are chosen
according to their level of dominance. For the last positions available in the
surviving population, a roulette strategy is employed, in which the
probability of selection is proportional to the _crowding-distance_ of an
individual. Our approach is summarized in Alg. 2.

Algorithm 2 MEPI

0: $\mathbf{x}_{g}$, $n_{\textrm{pop}}$, $n_{\textrm{ger}}$,
$n_{\textrm{cp}}$, $n_{\textrm{par}}$, $\varepsilon$

1: $k\leftarrow 0$

2: for each individual $i$ in the population do

3: Assign a random initial policy $\mathbf{u}^{(i)}$

4: Calculate the transformed time value $\bar{v}_{T}^{(i)}$ and transformed
energy value $\bar{v}_{E}^{(i)}$ using the $\mathbf{u}^{(i)}$ policy, as well
as their average values over the state space

5: while $k<n_{\textrm{ger}}$ do

6: $k\leftarrow k+1$

7: Calculate _crowding distance_ of the current population

8: Use _Policy Switching_ over all the population to generate elite
individuals with regards to the $\bar{v}_{T}^{(i)}$ and $\bar{v}_{E}^{(i)}$
value functions respectively

9: Considering a random $\alpha$, use _Policy Switching_ over all the
population to generate an elite individual with regards to the value functions
$\tilde{v}^{(i)}(\mathbf{x})=\dfrac{\alpha_{i}\bar{v}_{T}^{(i)}+(1\\!-\\!\alpha_{i})\bar{v}_{E}^{(i)}-\bar{v}_{T}^{(i)}\bar{v}_{E}^{(i)}}{1-(1\\!-\\!\alpha_{i})\bar{v}_{T}^{(i)}-\alpha_{i}\bar{v}_{E}^{(i)}}$
considering an _additional_ set of fixed control actions.

10: Generate $n_{\textrm{cp}}$ new offspring individuals using _Policy
Switching_ with $n_{\textrm{par}}$ parents chosen using a binary tournament
(decided by _dominance_ and _crowding distance_) and a gaussian mutation.

11: Generate $n_{\textrm{pop}}-n_{\textrm{cp}}-3$ individuals using the simple
_cross-over_ in (22) with 2 parents chosen using a binary tournament (decided
by _dominance_ and _crowding distance_) and a gaussian mutation.

12: Considering the individuals from the original population and the offspring
population, sort them using the _fast non-dominated sorting_ and calculate
their _crowding distance_

13: Select individuals to form the next generation of the population according
to their _dominance level_. Employ a roulette (using the _crowding distance_)
to fill the available spots when the remaining individuals cannot be chosen
with dominance only.

14: return Population $\mathbf{u}^{(i)}$, and value functions
$\bar{v}_{T}^{(i)}$ and $\bar{v}_{E}^{(i)}$

##  IV Numerical simulations

###  IV-A Example 1

As the first example, we consider an agent with simple linear drift dynamics

$\displaystyle\begin{bmatrix}\dot{x}_{1}\\\ \dot{x}_{2}\\\
\dot{x}_{3}\end{bmatrix}$ $\displaystyle=\begin{bmatrix}-1&1.2094&0.6937\\\
-1.2094&-1&2.6564\\\ -0.6937&-2.6564&-1\end{bmatrix}\begin{bmatrix}x_{1}\\\
x_{2}\\\ x_{3}\end{bmatrix}$
$\displaystyle+\begin{bmatrix}-0.2415&0.3971&0.8855\\\
-0.9701&-0.0744&-0.2312\\\
-0.0259&-0.9148&0.4031\end{bmatrix}\begin{bmatrix}v_{1}\\\ v_{2}\\\
v_{3}\end{bmatrix},$ (23)

with $x_{1}$, $x_{2}$ and $x_{3}\in[-1,1]$ being the position on the space,
and $v_{1}$, $v_{2}$ and $v_{3}\in[-2,2]$ the control inputs. The control
objective is to drive the agent from any state $(x_{1},x_{2},x_{3})$ to the
goal position $(-0.2,0.2,0)$. This problem was solved using the CPI (Alg. 1)
and the MEPI (Alg. 2), and in both cases, the space was discretized by a
structured grid of 1332 points (with the grid points equally distributed in
space, and with the goal added to the grid). The step-size used for the
Trapezoidal method was $\Delta t=0.1$.

For the CPI, we considered 14 logarithmically spaced values of $\alpha_{i}$
ranging from $0.01$ to $1.0$, and the allowable inputs were discretized
considering 9 points in each direction ($v_{1}$, $v_{2}$ and $v_{3}$), leading
to 729 possible control actions. The algorithm ran for 15 iterations to find
the result presented in Fig. 1.

Figure 1: Result running the _Concurrent Policy Iteration_ for Example 1. The
top plot represents the solution set found, while the bottom plot shows how
each solution corresponds to a different path in state space, for initial
point $(-0.9,-0.9,0.9)$. The red arrows represent the flow vector field, and
the solutions are color-coded to match each trajectory in state space with a
point on the objective function space, with yellow being the fastest
trajectory and red the trajectory that spends the least amount of energy.

For the MEPI, we considered a population of 20 individuals, with
$n_{cp}=15,n_{par}=3$ and $\sigma=0.2$ as the standard deviation for the
Gaussian mutation. The initial population was randomly chosen using a uniform
distribution over the allowable controls. The algorithm ran for 60 generations
to find the result presented in Figure 2.

Figure 2: Result running the _Multiobjective Evolutionary Policy Iteration_
for Example 1. The top plot represents the final population found while the
bottom plot shows how each solution corresponds to a different path in state
space, for initial point $(-0.9,-0.9,0.9)$. The red arrows represent the flow
vector field, and the solutions are color-coded in order to match each
trajectory in state space with a point on the objective function space, with
green being the fastest trajectory and blue the trajectory that spends the
least amount of energy.

For this example, both algorithms were able to find similar solutions. It is
important to note that, even though for this example the CPI was able to find
a set of solutions that seem to be well-distributed in the objective space,
that is not always the case. Finding a suitable set of $\alpha_{i}$ values can
be a problem at times. This problem does not happen with MEPI since it ranks
solutions based on the _crowding distance_ metric.

###  IV-B Example 2 - scenario with obstacles

As a second example, we consider an agent whose drift dynamics approximate a
vortex with constant flow velocity centered at $(0.5,0)$, and complete
actuation. The dynamics can be described by:

$\displaystyle\bar{\mathbf{f}}(x,y)$ $\displaystyle=\begin{bmatrix}-1&3\\\
-3&-1\end{bmatrix}\begin{bmatrix}x-0.5\\\ y\end{bmatrix},$
$\displaystyle\begin{bmatrix}\dot{x}\\\ \dot{y}\end{bmatrix}$
$\displaystyle=\dfrac{1}{0.01+\|\bar{\mathbf{f}}(x,y)\|_{2}}\bar{\mathbf{f}}(x,y)+\begin{bmatrix}v_{x}\\\
v_{y}\end{bmatrix},$ (24)

with $x$ and $y\in[-1,1]$ representing the position on the plane, and $v_{x}$
and $v_{y}\in[-2,2]$ the agent’s velocities concerning the drift field, used
as control inputs. The control objective is to drive the agent from any state
$(x,y)$ to the goal position $(-0.5,0.6)$, while avoiding the obstacle. Once
again this problem was solved by employing both, CPI and MEPI. The space was
originally discretized by 596 points, however, with this discretization, the
CPI was not capable of finding suitable solutions. Therefore, in this example,
the MEPI used 596 points to discretize the state space, while CPI used 796
points (in both cases these points include the desired goal position and 95
points describing the boundaries of the obstacle). The step size of the
trapezoidal method used was $\Delta t=0.05$ for both methods.

For the CPI, we considered 15 logarithmically spaced values of $\alpha_{i}$
ranging from $0.01$ to $1$, and the allowable inputs were discretized
considering 15 points in each direction ($v_{x}$ and $v_{y}$), leading to 225
possible control actions. The algorithm ran for 30 iterations to find the
result presented in Figure 3.

Figure 3: Result found when running _Concurrent Policy Iteration_ for Example
2. The top plot represents the set of solutions found while the bottom plot
shows how each solution corresponds to a different path in state space, for
initial point $(0,0.9)$. The blue arrows represent the flow vector field, the
black region represent an obstacle, and the solutions are color-coded to match
each trajectory in state space with a point on the objective function space,
with yellow being the fastest trajectory and red the trajectory that spends
the least amount of energy.

For MEPI, we considered a population with 20 individuals, with
$n_{cp}=15,n_{par}=3$, and $\sigma=0.2$ as the standard deviation for the
Gaussian mutation. The initial population was randomly chosen using a uniform
distribution over the allowable controls. The algorithm ran for 100
generations to find the result presented in Figure 4.

Figure 4: Result found when running _Multiobjective Evolutionary Policy
Iteration_ for Example 2. The top plot represents the final population found
while the bottom plot shows how each solution corresponds to a different path
in state space, for initial point $(0,0.9)$. The blue arrows represent the
flow vector field, the black region represent an obstacle, and the solutions
are color-coded to match each trajectory in state space with a point on the
objective function space, with green being the fastest trajectory and blue the
trajectory that spends the least amount of energy.

For this example, unlike with Example 1, the solutions were considerably
different (looking at the objective function space) and the solutions found
with MEPI have a substantially smaller energy cost than the ones from CPI.
This can be explained by the fact that CPI uses a discrete set of control
actions, while MEPI is capable of using a continuous set. It is also
interesting to note that, with both methods, the optimal time paths chose the
shortest path to the goal, while the energy optimal path tries to make the
most use of the drift dynamics possible.

###  IV-C Example 3 - marine navigation problem

In this last example, we consider a problem in which the agent can be regarded
as a marine vessel moving through an ocean environment. In this case, the flow
vector field describes the ocean currents, whereas the steering matrix
describes the allowable input velocity directions for the vehicle.

Figure 5: Result found when running _Concurrent Policy Iteration_ for Example
3. The top plot represents the set of solutions found while the bottom plot
shows how each solution corresponds to a different path in state space, for
initial point $(16,50)$. The blue arrows represent the flow vector field, the
black regions represent obstacles, and the solutions are color-coded to match
each trajectory in state space (on the right) with a point on the objective
function space (on the left), with yellow being the fastest trajectory and red
the trajectory that spends the least amount of energy. Figure 6: Result found
when running _Multiobjective Evolutionary Policy Iteration_ for Example 3. The
top plot represents the final population found while the bottom plot shows how
each solution corresponds to a different path in state space, for the initial
point $(16,50)$. The blue arrows represent the flow vector field, the black
regions represent the obstacles, and the solutions are color-coded in order to
match each trajectory in state space with a point on the objective function
space, with green being the fastest trajectory and blue the trajectory that
spends the least amount of energy.

Inspired by [23, 16] we consider an ocean current model in $\mathds{{R}}^{2}$
given by the superposition of different one-point vortex solutions called
_viscous Lamb vortices_ , given by

$\footnotesize\mathbf{f}_{i}(\mathbf{x})=\Gamma_{i}\begin{bmatrix}-\dfrac{y-c_{2i}}{2\pi(\mathbf{x}-\mathbf{c}_{i})^{T}(\mathbf{x}-\mathbf{c}_{i})}\left(1-e^{-\frac{(\mathbf{x}-\mathbf{c}_{i})^{T}(\mathbf{x}-\mathbf{c}_{i})}{\delta_{i}^{2}}}\right)\\\
\dfrac{x-c_{1i}}{2\pi(\mathbf{x}-\mathbf{c}_{i})^{T}(\mathbf{x}-\mathbf{c}_{i})}\left(1-e^{-\frac{(\mathbf{x}-\mathbf{c}_{i})^{T}(\mathbf{x}-\mathbf{c}_{i})}{\delta_{i}^{2}}}\right)\end{bmatrix},$
(25)

in which, $\mathbf{x}=(x,y)$ is the vessel position,
$\mathbf{c}_{i}=(c_{1i},c_{2i})$ is the center of the $i$th vortex, and
$\delta_{i}$ and $\Gamma_{i}$ are parameters related to its radius and
strength. In this example, we consider that four vortices are used, so that
the agent dynamics can be described by

$\displaystyle\begin{bmatrix}\dot{x}\\\
\dot{y}\end{bmatrix}=\left(\sum_{i=1}^{4}\mathbf{f}_{i}(\mathbf{x})\right)+\begin{bmatrix}v_{x}\\\
v_{y}\end{bmatrix},$ (26)

with $v_{x}\in[-3,3]$ and $v_{y}\in[-3,3]$ being the input velocities, and
parameters $\Gamma_{1}=-50$, $\Gamma_{2}=\Gamma_{3}=\Gamma_{4}=50$,
$\delta_{1}=\delta_{2}=\delta_{3}=\delta_{4}=10$, $\mathbf{c}_{1}=(20,30)$,
$\mathbf{c}_{2}=(60,70)$, $\mathbf{c}_{3}=(27,65)$, and
$\mathbf{c}_{4}=(60,30)$. The environment is a 100m x 100m square
($x\in[0,100]$, $y\in[0,100]$) with island shaped objects and is depicted in
Figures 5 and 6. The goal is to drive the vessel from any point $(x,y)$ in
this area to the goal position $(80,80)$ while avoiding the obstacles.

Similarly as the other examples, this problem was solved by employing CPI and
MEPI. In both cases, the space was discretized by an unstructured grid of 3412
points and the step size used for the Trapezoidal method was $\Delta t=1$.

For CPI, we considered 15 logarithmically spaced values of $\alpha_{i}$
ranging from $0.01$ to $1$, and the allowable inputs were discretized
considering 15 points in each direction ($v_{x}$ and $v_{y}$), leading to 225
possible control actions. The algorithm ran for 30 iterations to find the
result presented in Figure 5.

For MEPI, we considered a population with 20 individuals, with
$n_{cp}=15,n_{par}=3$ and $\sigma=0.2$ as the standard deviation for the
Gaussian mutation. The initial population was randomly chosen using a uniform
distribution over the allowable controls. The algorithm ran for 60 generations
to find the result presented in Figure 6.

Similarly to Example 1, the solutions found in this example were comparable
for both methods (though the concentration of points in the objective space
was a bit different in both cases). When considering the extremes of the
Pareto Set of solutions found, CPI was able to find the best time-optimal
solution, whereas MEPI was able to find the best energy-optimal solution.

It is also interesting to note that, with both methods, when moving along the
efficient solutions they can be _visually_ divided into 3 groups of paths
(considering the trajectories taken in a closed loop).

##  V Conclusion and Future Work

Path planning approaches are of paramount importance in Robotics, as they
enable autonomous robots to navigate complex and dynamic environments with
efficiency and safety. This paper proposes a multi-objective path planning
formulation to determine paths that simultaneously optimize travel time and
energy consumption.

The presented methodology incorporates the dynamic influence of environmental
flow fields and considers obstacles and forbidden zones. Our approach relies
on the proposed Harmonic Transformation, which maps values onto a specific
range, effectively mitigating potential numerical issues.

In order to determine the set of Pareto efficient solutions within the context
of multi-objective optimization, we present two distinct approaches. The first
approach is deterministic, involving the simultaneous solution of multiple
single-objective optimizations. This deterministic method capitalizes on the
parallel resolution of individual objectives to achieve Pareto efficiency. The
second approach takes on an evolutionary perspective. By employing principles
of evolution and selection, this evolutionary approach explores the solution
space to uncover Pareto optimal solutions more adaptively and comprehensively.

In future work, trying to overcome some of the computational limitations of
this approach, we intend to study the use of adaptive grids [24], as well as
how this problem could be handled in a reinforcement learning context.

###  -A Proof of Theorem 1

The proof of Theorem 1 is divided into two parts. The first one proves that
the HJB equation admits a comparison principle and a unique viscosity
solution, whereas the second part employs the Barles-Souganidis Theorem [25,
Theorem 2.1] to show that the proposed SL numerical scheme converges to this
viscosity solution.

####  -A1 Viscosity solution - existence and uniqueness

For the HJB equation in (15), we can write the Hamiltonian

$H(\mathbf{x},v,\mathbf{p})=\sup_{\mathbf{u}\in\mathcal{U}}\left\\{-\
\mathbf{p}\cdot\mathbf{f}(\mathbf{x},\mathbf{u})-\left(1-v\right)^{2}\ell(\mathbf{x},\mathbf{u})\right\\},$

which is easily shown to be _uniformly continuous in_ $\mathbf{x},v$ and
$\mathbf{p}$, _convex in_ $\mathbf{p}$ and _monotone in_ $v$ if $\mathbf{f}$
is Lipschitz and $\ell$ is _positive semidefinite_ and Lipschitz.

In addition to this, considering that $f(.,\mathbf{u})$ and
$\ell(.,\mathbf{u})$ are Lipschitz in $\mathbf{x}$ (with _moduli of
continuity_ $L_{1}$ and $L_{2}$ independent of $\mathbf{u}$), it follows that
(considering that the $\sup$ is attained by $\mathbf{u}$ for
$H(\mathbf{y},v,\mathbf{p})$)

$\displaystyle H(\mathbf{x},v,\mathbf{p})-H(\mathbf{y},v,\mathbf{p})$
$\displaystyle\leq\mathbf{p}\cdot\left(\mathbf{f}(\mathbf{y},\mathbf{u})-\mathbf{f}(\mathbf{x},\mathbf{u})\right)$
$\displaystyle+(1-v)^{2}\left(\ell(\mathbf{y},\mathbf{u})-\ell(\mathbf{x},\mathbf{u})\right),$
$\displaystyle H(\mathbf{x},v,\mathbf{p})-H(\mathbf{y},v,\mathbf{p})$
$\displaystyle\leq
L_{1}\|p\|\|\mathbf{x}-\mathbf{y}\|+L_{2}\|\mathbf{x}-\mathbf{y}\|,$
$\displaystyle H(\mathbf{x},v,\mathbf{p})-H(\mathbf{y},v,\mathbf{p})$
$\displaystyle\leq L_{3}\left(1+\|p\|\right)\|\mathbf{x}-\mathbf{y}\|,$

with $L_{3}=\max(L_{1},L_{2})$. Since a similar bound can be similarly found
for the other difference, it follows that

$\displaystyle|H(\mathbf{x},v,\mathbf{p})-H(\mathbf{y},v,\mathbf{p})|$
$\displaystyle\leq L_{3}\left(1+\|p\|\right)\|\mathbf{x}-\mathbf{y}\|,$

and, according to [19, Theorem 2.13], the viscosity sub and supersolutions of
(15) admit a _comparison principle_ , and, as such, equation (15) has a unique
_viscosity solution_.

####  -A2 Convergence analysis

Having shown that equation (15) admits a unique viscosity solution, as well as
a _comparison principle_ , from the Barles-Souganidis Theorem [25, Theorem
2.1], it suffices to show that the proposed numerical scheme is _monotone_ , a
_contraction mapping_ and _consistent_ to ensure its convergence to the
_viscosity solution_.

$\rightarrow$_Monotonicity_

Consider two functions $\overline{W}$ and $\overline{V}$, with
$\overline{W}\leq\overline{V}$ for every point on the grid. Suppose that the
$\inf$ operator in (11) is attained by $\overline{\mathbf{w}}$ for
$\overline{W}$, and $\overline{\mathbf{u}}$ for $\overline{V}$. It follows
that:

$\displaystyle\overline{W}_{k}(\mathbf{x}_{k})\leq\dfrac{\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]+\left(1-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}{1+\left(1-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})},$
$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\overline{W}_{k}(\mathbf{x}_{k})\geq$
$\displaystyle\quad\dfrac{\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]+\left(1-\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}{1+\left(1-\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}$
$\displaystyle\quad-\dfrac{\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]+\left(1-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}{1+\left(1-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})},$
$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\overline{W}_{k}(\mathbf{x}_{k})\geq\dfrac{\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]}{\Omega_{\overline{V}}(\mathbf{x}_{k},\overline{\mathbf{u}})\Omega_{\overline{W}}(\mathbf{x}_{k},\overline{\mathbf{u}})}$
(27)

with

$\displaystyle\Omega_{\overline{V}}(\mathbf{x}_{k},\mathbf{u})$
$\displaystyle=\left(1+\left(1-\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u})\right)$
(28) $\displaystyle\Omega_{\overline{W}}(\mathbf{x}_{k},\mathbf{u})$
$\displaystyle=\left(1+\left(1-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]\right)g(\mathbf{x}_{k},\mathbf{u})\right)$
(29)

which implies that
$\overline{V}_{k}(\mathbf{x}_{k})-\overline{W}_{k}(\mathbf{x}_{k})\geq 0$
because we employed a linear interpolation, and
$\Omega_{\overline{V}}(\mathbf{x}_{k},\overline{\mathbf{u}})>0$,
$\Omega_{\overline{W}}(\mathbf{x}_{k},\overline{\mathbf{u}})>0$, since
$\overline{V}\leq 1,$ $\overline{W}\leq 1,$
$g(\mathbf{x}_{k},\overline{\mathbf{u}})>0$.

$\rightarrow$_Contractiveness_

Considering two functions $\overline{W}$ and $\overline{V}$, with
$\overline{\mathbf{w}}$ minimizing $\overline{W}$. It follows that:

$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\overline{W}_{k}(\mathbf{x}_{k})$
$\displaystyle\leq\dfrac{\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]-\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]}{\Omega_{\overline{V}}(\mathbf{x}_{k},\overline{\mathbf{w}})\Omega_{\overline{W}}(\mathbf{x}_{k},\overline{\mathbf{w}})}.$

Note that, for the case in which both
$\mathcal{I}_{\overline{V}_{k+1}}[\mathbf{x}_{k+1}]$ and
$\mathcal{I}_{\overline{W}_{k+1}}[\mathbf{x}_{k+1}]$ are equal to 1, the
inequality is trivially upper bounded by anything larger than zero. As such,
we can consider, as a worst-case bound, the case in which one of them is one
and the other is $1-\varepsilon$, leading to

$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\overline{W}_{k}(\mathbf{x}_{k})$
$\displaystyle\leq\dfrac{1}{1+\varepsilon
g(\mathbf{x}_{k},\overline{\mathbf{w}})}\|\overline{V}-\overline{W}\|_{\infty}^{(k+1)},$

with $\|\overline{V}-\overline{W}\|_{\infty}^{(k+1)}$ being the maximum of the
error between the two functions in the next time step. Since we are assuming
that $\ell(\mathbf{x},\mathbf{u})>0$, then
$g(\mathbf{x}_{k},\mathbf{u}_{k})>\overline{g}\Delta
t>0~{}~{}\forall\mathbf{x}_{k},\mathbf{u}_{k}$, and a similar bound can be
found for $\overline{W}_{k}(\mathbf{x}_{k})-\overline{V}_{k}(\mathbf{x}_{k})$,
then:

$\displaystyle\|\overline{V}-\overline{W}\|_{\infty}^{(k)}$
$\displaystyle\leq\dfrac{1}{1+\varepsilon\overline{g}\Delta
t}\|\overline{V}-\overline{W}\|_{\infty}^{(k+1)}.$ (30)

As we solve the problem back in time, this shows that our approximation scheme
is a contraction mapping. From the Banach fixed-point Theorem, it guarantees
that our approximation scheme converges to a unique solution.

$\rightarrow$_Consistency_

We start our consistency analysis by considering the error of time
discretization, comparing solutions from (7) and (10). If we consider that the
$\inf$ operator is attained by $\overline{\mathbf{u}}$ in (10), it follows
that:

$\displaystyle\bar{v}(\mathbf{x}(t))-\bar{v}_{k}(\mathbf{x}_{k})\leq$
$\displaystyle\quad\dfrac{\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))+\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}}),\overline{\mathbf{u}})dt}{1+\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}}),\overline{\mathbf{u}})dt}$
$\displaystyle\quad-\dfrac{\bar{v}_{k}(\mathbf{x}_{k+1})+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}{1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})}.$

Some algebraic manipulations lead to:

$\displaystyle\bar{v}(\mathbf{x}(t))-\bar{v}_{k}(\mathbf{x}_{k})\leq$
$\displaystyle\quad\dfrac{\left(\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))-\bar{v}_{k}(\mathbf{x}_{k+1})\right)}{\left(1+q(\mathbf{x}(t),\overline{\mathbf{u}})\right)\left(1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})\right)}$
$\displaystyle\quad+\dfrac{\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))\right)\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)\Delta
g}{\left(1+q(\mathbf{x}(t),\overline{\mathbf{u}})\right)\left(1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\overline{\mathbf{u}})\right)},$
$\displaystyle
q(\mathbf{x}(t),\overline{\mathbf{u}})=\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}}),\overline{\mathbf{u}})dt,$
$\displaystyle\Delta g=\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}}),\overline{\mathbf{u}})dt-g(\mathbf{x}_{k},\overline{\mathbf{u}}).$

From [26, Lemma 406B], we know that, using a trapezoidal method to solve the
system dynamics in (1) leads to a bound (over one time step)

$\displaystyle\|\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}})-\mathbf{x}_{k+1}\|\leq LM\Delta t^{2},$

with $L$ the Lipschitz constant of $\mathbf{f}(\mathbf{x},\mathbf{u})$ with
regards to $\mathbf{x}$, and $\|\mathbf{f}(\mathbf{x},\mathbf{u})\|\leq M$.
Note that tighter bounds are available for the local truncation error of the
trapezoidal method (the error over one time-step), but these usually require
extra assumptions about the smoothness of $\mathbf{f}$. In addition to this,
from [27, Corollary 1.4], considering that $\ell(t)$ is Lipschitz, we have
that

$\displaystyle\left|\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}})dt-g(\mathbf{x}_{k},\overline{\mathbf{u}})\right|$
$\displaystyle\quad\leq\left|\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\overline{\mathbf{u}})dt-\dfrac{\left(\ell(\mathbf{x}_{k},\bar{\mathbf{u}})+\ell(\mathbf{y}_{x}(\Delta
t,\bar{\mathbf{u}}),\bar{\mathbf{u}})\right)}{2}\right|$
$\displaystyle\quad+\left|\dfrac{\left(\ell(\mathbf{x}_{k+1},\bar{\mathbf{u}})-\ell(\mathbf{y}_{x}(\Delta
t,\bar{\mathbf{u}}),\bar{\mathbf{u}})\right)}{2}\right|$
$\displaystyle\quad\leq\dfrac{\Delta
t^{2}}{8}\left(\sup\dot{\ell}-\inf\dot{\ell}\right)+KLM\Delta t^{2},$

with $K$ being the Lipschitz constant of $\ell$ with regards to $\mathbf{x}$,
and $C_{1}$ some constant.

If we consider that, $\bar{v}$ is Lipschitz continuous, it follows that

$\displaystyle\left|\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\bar{\mathbf{u}}),\bar{\mathbf{u}})dt-g(\mathbf{x}_{k},\bar{\mathbf{u}})\right|$
$\displaystyle\leq C_{1}\Delta t^{2},$
$\displaystyle\left|\bar{v}(\mathbf{y}_{x}(\Delta
t,\overline{\mathbf{u}}))-\bar{v}(\mathbf{x}_{k+1})\right|$ $\displaystyle\leq
C_{2}\Delta t^{2},$ (31)

and that the worst case upper bound for (-A2) happens when one of the value
functions is equal to 1, while the other is equal to $1-\varepsilon$, it
follows that

$\displaystyle\bar{v}(\mathbf{x}(t))-\bar{v}_{k}(\mathbf{x}_{k})\leq$
$\displaystyle\dfrac{C_{3}\Delta
t^{2}+\|\bar{v}-\bar{v}_{k}\|_{\infty}}{1+\varepsilon\bar{g}\Delta t}.$ (32)

If we consider that the $\inf$ operator is attained by $\mathbf{u}^{\ast}$ in
(7), and that

$\displaystyle\hat{\mathbf{u}}_{k}=\frac{1}{\Delta t}\int_{0}^{\Delta
t}\mathbf{u}^{\ast}(\tau)d\tau$ (33)

is the control obtained by the mean of the optimal control over a time step,
it follows that

$\displaystyle\bar{v}_{k}(\mathbf{x}_{k})-\bar{v}(\mathbf{x}(t))\leq\dfrac{\bar{v}_{k}(\mathbf{x}_{k+1})+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})}{1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})}$
$\displaystyle\;-\dfrac{\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))+\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt}{1+\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt}.$

Some algebraic manipulations lead to:

$\displaystyle\bar{v}_{k}(\mathbf{x}_{k})-\bar{v}(\mathbf{x}(t))\leq$
$\displaystyle\;\dfrac{\left(\bar{v}_{k}(\mathbf{x}_{k+1})-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))\right)}{\left(1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})\right)\left(1+\bar{q}(\mathbf{x}(t),\mathbf{u}^{\ast})\right)}$
$\displaystyle\;+\dfrac{\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))\right)\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)\Delta\hat{g}}{\left(1+\left(1-\bar{v}_{k}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})\right)\left(1+\bar{q}(\mathbf{x}(t),\mathbf{u}^{\ast})\right)},$
(34)
$\displaystyle\bar{q}(\mathbf{x}(t),\mathbf{u}^{\ast})=\left(1-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))\right)\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt,$ (35)
$\displaystyle\Delta\hat{g}=\left(g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt\right).$

Consider that

$\displaystyle\mathbf{y}_{x}(\Delta t,\mathbf{u}^{\ast})$
$\displaystyle=\mathbf{x}_{k}+\int_{0}^{\Delta
t}\mathbf{f}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))d\tau,$
$\displaystyle\mathbf{x}_{k+1}$ $\displaystyle=\mathbf{x}_{k}+\frac{\Delta
t}{2}\left(\mathbf{f}(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})+\mathbf{f}(\mathbf{x}_{k+1},\hat{\mathbf{u}}_{k})\right),$

since $\mathbf{f}$ can be decomposed, as in (2), it follows that

$\displaystyle\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})=\mathbf{x}_{k}+\int_{0}^{\Delta
t}\mathbf{f}_{1}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k+1})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau$
$\displaystyle+\dfrac{F_{2}(\mathbf{x}_{k})}{2}\int_{0}^{\Delta
t}\mathbf{u}^{\ast}(\tau)d\tau+\dfrac{F_{2}(\mathbf{x}_{k+1})}{2}\int_{0}^{\Delta
t}\mathbf{u}^{\ast}(\tau)d\tau,$ $\displaystyle\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})=\mathbf{x}_{k}+\int_{0}^{\Delta
t}\mathbf{f}_{1}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k+1})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau$
$\displaystyle+\dfrac{F_{2}(\mathbf{x}_{k})\Delta
t}{2}\hat{\mathbf{u}}_{k}+\dfrac{F_{2}(\mathbf{x}_{k+1})\Delta
t}{2}\hat{\mathbf{u}}_{k},$ $\displaystyle\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}=\int_{0}^{\frac{\Delta
t}{2}}\left(\mathbf{f}_{1}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{f}_{1}(\mathbf{x}_{k})\right)d\tau$
$\displaystyle+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left(\mathbf{f}_{1}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{f}_{1}(\mathbf{x}_{k+1})\right)d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau$
$\displaystyle+\int_{0}^{\Delta
t}\dfrac{\left(F_{2}(\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-F_{2}(\mathbf{x}_{k+1})\right)}{2}\mathbf{u}^{\ast}(\tau)d\tau.$

If we consider that $\mathbf{f}_{1}$ and $F_{2}$ are Lipschitz in
$\mathbf{x}$, and that $\mathbf{u}$ is bounded, it follows that

$\displaystyle\|\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|\leq L_{1}\int_{0}^{\frac{\Delta
t}{2}}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k}\|d\tau$
$\displaystyle+L_{1}\int_{\frac{\Delta t}{2}}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k+1})\|d\tau$
$\displaystyle+L_{2}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k}\|d\tau$
$\displaystyle+L_{2}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k+1})\|d\tau,$
$\displaystyle\|\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|\leq L_{1}\int_{0}^{\frac{\Delta
t}{2}}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k}\|d\tau$
$\displaystyle+L_{1}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k+1})\|d\tau$
$\displaystyle+L_{2}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k}\|d\tau$
$\displaystyle+L_{2}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast}(\tau))-\mathbf{x}_{k+1})\|d\tau.$

Considering that $\|\mathbf{f}\|\leq M,\forall\mathbf{x},\mathbf{u}$, it
follows that

$\displaystyle\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast})-\mathbf{x}_{k}\|\leq
M\tau,$

and we arrive at the bound

$\displaystyle\|\mathbf{y}_{x}(\Delta t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|$
$\displaystyle\quad\leq(L_{1}+L_{2})\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|d\tau$
$\displaystyle\quad+\frac{L_{1}M\Delta t^{2}}{8}+\frac{L_{2}M\Delta t^{2}}{2}$
$\displaystyle\|\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|\leq\bar{L}\int_{0}^{\Delta
t}\|\mathbf{y}_{x}(\tau,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|d\tau$
$\displaystyle\quad+\bar{M}\Delta t^{2},$

which, from the integral form of the Gronwall-Bellman inequality leads to

$\displaystyle\|\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast})-\mathbf{x}_{k+1}\|\leq\bar{M}\Delta t^{2}e^{\bar{L}\Delta
t}.$

Considering that

$\displaystyle g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt=$
$\displaystyle\;\int_{0}^{\frac{\Delta
t}{2}}\left(\ell(\mathbf{x}_{k},\hat{\mathbf{u}}){\color[rgb]{1,0,0}-\ell(\mathbf{x}_{k},\mathbf{u}^{\ast})+\ell(\mathbf{x}_{k},\mathbf{u}^{\ast})}-\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})\right)dt$
$\displaystyle\;+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left(\ell(\mathbf{x}_{k+1},\hat{\mathbf{u}}){\color[rgb]{1,0,0}-\ell(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})}\right)$
$\displaystyle\;+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left({\color[rgb]{1,0,0}\ell(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})}-\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})\right)dt,$

and, considering that $\ell$ is convex in $\mathbf{u}$, from Jensen’s
inequality, it follows that

$\displaystyle-\int_{0}^{\frac{\Delta
t}{2}}\ell(\mathbf{x}_{k},\mathbf{u}^{\ast})dt\leq-\frac{\Delta
t}{2}\ell(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})$ (36)
$\displaystyle-\int_{\frac{\Delta t}{2}}^{\Delta t}\ell(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt\leq-\frac{\Delta
t}{2}\ell(\mathbf{y}_{x}(\Delta t,\mathbf{u}^{\ast}),\hat{\mathbf{u}}_{k}),$

so that

$\displaystyle g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt\leq$
$\displaystyle\quad\int_{0}^{\frac{\Delta
t}{2}}\left(\ell(\mathbf{x}_{k},\mathbf{u}^{\ast})-\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})\right)dt$
$\displaystyle\quad+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left(\ell(\mathbf{x}_{k+1},\hat{\mathbf{u}}_{k})-\ell(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}),\hat{\mathbf{u}}_{k})\right)dt$
$\displaystyle\quad+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left(\ell(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})-\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})\right)dt.$

Considering that $\ell$ has Lipschitz constant $K$ with respect to
$\mathbf{x}$, it follows that

$\displaystyle g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt\leq\int_{0}^{\frac{\Delta
t}{2}}KM\tau d\tau$ $\displaystyle\quad+\int_{\frac{\Delta t}{2}}^{\Delta
t}\left(K\bar{M}\Delta t^{2}e^{\bar{L}\Delta t}+KM\tau\right)d\tau$ (37)
$\displaystyle g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt\leq\frac{KM\Delta
t^{2}}{2}+\frac{K\bar{M}\Delta t^{3}}{2}e^{\bar{L}\Delta t}.$

If we consider that, $\bar{v}$ is Lipschitz continuous, it follows that

$\displaystyle g(\mathbf{x}_{k},\hat{\mathbf{u}}_{k})-\int_{0}^{\Delta
t}\ell(\mathbf{y}_{x}(t,\mathbf{u}^{\ast}),\mathbf{u}^{\ast})dt$
$\displaystyle\leq C_{4}\Delta t^{2}+C_{5}\Delta t^{3}e^{\bar{L}\Delta t}$
(38) $\displaystyle|\bar{v}(\mathbf{x}_{k+1})-\bar{v}(\mathbf{y}_{x}(\Delta
t,\mathbf{u}^{\ast}))|$ $\displaystyle\leq C_{6}\Delta t^{2}e^{\bar{L}\Delta
t}.$

Considering also that the worst case upper bound in (34) happens when one of
the value functions is equal to 1, while the other is equal to
$1-\varepsilon$, it follows that

$\displaystyle\bar{v}_{k}(\mathbf{x}_{k})-\bar{v}(\mathbf{x}(t))\leq$
$\displaystyle\quad\dfrac{C_{4}\Delta t^{2}+\left(C_{5}\Delta
t^{3}+C_{6}\Delta t^{2}\right)e^{\bar{L}\Delta
t}+\|\bar{v}-\bar{v}_{k}\|_{\infty}}{1+\varepsilon\bar{g}\Delta t}.$ (39)

By combining (32) and (39), we have that

$\displaystyle\|\bar{v}-\bar{v}_{k}\|_{\infty}$
$\displaystyle\leq\dfrac{C_{7}\Delta t^{2}+\left(C_{5}\Delta t^{3}+C_{6}\Delta
t^{2}\right)e^{\bar{L}\Delta t}}{\varepsilon\bar{g}\Delta t}$ (40)
$\displaystyle\approx\dfrac{C_{7}\Delta t^{2}+\left(C_{5}\Delta
t^{3}+C_{6}\Delta t^{2}\right)(1+\bar{L}\Delta t)}{\varepsilon\bar{g}\Delta
t},$

in which the last approximation comes from considering that, for $\theta$
close to zero, $e^{\theta}\approx 1+\theta$. Asymptotically, this bound is
_dominated_ by the linear term when $\Delta t\rightarrow 0$, since the higher
order terms vanish faster. In that regard, we will write

$\displaystyle\|\bar{v}-\bar{v}_{k}\|_{\infty}\leq C\Delta t.$ (41)

For the space discretization error, we analyze the errors of the value
function on the grid points, by comparing (10) and (11). To differentiate
them, we will denote the value on the grid points of (11) by
$\overline{V}_{k}$. If we consider that the $\inf$ is attained by
$\overline{\mathbf{u}}$ in (10), it follows that:

$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\bar{v}_{k}(\mathbf{x}_{k})\leq$
$\displaystyle\dfrac{\mathcal{I}_{\bar{V}_{k+1}}[\mathbf{x}_{k+1}]-\bar{v}_{k+1}(\mathbf{x}_{k+1})}{\Omega_{\overline{V}}(\mathbf{x}_{k},\overline{\mathbf{u}})\Omega_{\bar{v}}(\mathbf{x}_{k},\overline{\mathbf{u}})},$

with $\Omega_{\overline{V}}$ from (28) and

$\displaystyle\Omega_{\bar{v}}(\mathbf{x}_{k},\overline{\mathbf{u}})=1+\left(1-\bar{v}_{k+1}(\mathbf{x}_{k+1})\right)g(\mathbf{x}_{k},\overline{\mathbf{u}}).$

By considering that $\overline{V}$ is Lipschitz,
$g(\mathbf{x}_{k},\overline{\mathbf{u}})>\overline{g}\Delta t$, and that the
worst case upper bound happens when one of the value functions is equal to 1,
while the other is equal to $1-\varepsilon$ leads to:

$\displaystyle\overline{V}_{k}(\mathbf{x}_{k})-\bar{v}_{k}(\mathbf{x}_{k})\leq$
$\displaystyle\dfrac{C_{8}\Delta
x+\|\overline{V_{k}}-\overline{v}_{k}\|_{\infty}}{1+\varepsilon\overline{g}\Delta
t}.$

with $C_{8}$ being a constant and $\Delta x$ being the largest distance
between any point and a grid point.

Since a similar bound can be found for
$\bar{v}_{k}(\mathbf{x}_{k})-\overline{V}_{k}(\mathbf{x}_{k})$, then:

$\displaystyle\|\overline{V_{k}}-\overline{v}_{k}\|_{\infty}\leq$
$\displaystyle\dfrac{C_{8}\Delta
x+\|\overline{V_{k}}-\overline{v}_{k}\|_{\infty}}{1+\varepsilon\overline{g}\Delta
t}$ $\displaystyle\|\overline{V_{k}}-\overline{v}_{k}\|_{\infty}\leq$
$\displaystyle C_{9}\dfrac{\Delta x}{\Delta t}.$ (42)

with $C_{9}$ a constant. Combining (41) and (42), we have that:

$\displaystyle\|\overline{v}-\overline{V}\|_{\infty}\leq C_{10}\Delta
t+C_{9}\frac{\Delta x}{\Delta t},$ (43)

with $C_{10}$ a constant. Similarly to the original Kruzkov transformation,
the best coupling between $\Delta t$ and $\Delta x$, in this case, is given by
$\Delta x=\Delta t^{2}$, indicating that our grid resolution should be finer
than our time discretization resolution.

Since we have shown that our scheme is monotone, a contraction mapping, and
consistent, from the Barles-Souganidis Theorem, we have proven that it is
convergent. ∎

## References

  * [1] S. M. LaValle, _Planning Algorithms_. New York, NY, USA: Cambridge University Press, 2006.
  * [2] N. Hohmann, M. Bujny, J. Adamy, and M. Olhofer, “Hybrid Evolutionary Approach to Multi-objective Path Planning for UAVs,” in _IEEE Symp. Series on Computational Intelligence (SSCI)_ , 2021, pp. 1–8.
  * [3] M. N. Zafar and J. Mohanta, “Methodology for Path Planning and Optimization of Mobile Robots: A Review,” _Procedia Computer Science_ , vol. 133, pp. 141–152, 2018, international Conference on Robotics and Smart Manufacturing (RoSMa2018).
  * [4] E. S. Low, P. Ong, and K. C. Cheah, “Solving the optimal path planning of a mobile robot using improved Q-learning,” _Robotics and Autonomous Systems_ , vol. 115, pp. 143–161, 2019.
  * [5] A. Gasparetto, P. Boscariol, A. Lanzutti, and R. Vidoni, _Path Planning and Trajectory Planning Algorithms: A General Overview_. Cham: Springer International Publishing, 2015, pp. 3–27.
  * [6] P. Foehn, A. Romero, and D. Scaramuzza, “Time-optimal planning for quadrotor waypoint flight,” _Science Robotics_ , vol. 6, no. 56, p. eabh1221, 2021\.
  * [7] F. Ahmed and K. Deb, “Multi-objective optimal path planning using elitist non-dominated sorting genetic algorithms,” _Soft Computing_ , vol. 17, no. 7, pp. 1283–1299, Jul 2013.
  * [8] Y. Xue and J.-Q. Sun, “Solving the Path Planning Problem in Mobile Robotics with the Multi-Objective Evolutionary Algorithm,” _Applied Sciences_ , vol. 8, no. 9, 2018. [Online]. Available: https://www.mdpi.com/2076-3417/8/9/1425
  * [9] D. G. Macharet, A. Alves Neto, and D. Shishika, “Minimal Exposure Dubins Orienteering Problem,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 2280–2287, 2021.
  * [10] K. Cole and A. M. Wickenheiser, “Reactive trajectory generation for multiple vehicles in unknown environments with wind disturbances,” _IEEE Trans. on Robotics_ , vol. 34, no. 5, pp. 1333–1348, 2018.
  * [11] K. Weekly, A. Tinka, L. Anderson, and A. M. Bayen, “Autonomous river navigation using the hamilton–jacobi framework for underactuated vehicles,” _IEEE Trans. on Robotics_ , vol. 30, no. 5, pp. 1250–1255, 2014\.
  * [12] D. Kularatne, S. Bhattacharya, and M. A. Hsieh, “Going with the flow: a graph based approach to optimal path planning in general flows,” _Autonomous Robots_ , vol. 42, pp. 1369–1387, 2018.
  * [13] A. Alvarez, A. Caiti, and R. Onken, “Evolutionary path planning for autonomous underwater vehicles in a variable ocean,” _IEEE J. of Oceanic Engineering_ , vol. 29, no. 2, pp. 418–429, 2004.
  * [14] K. C. To, K. M. B. Lee, C. Yoo, S. Anstee, and R. Fitch, “Streamlines for motion planning in underwater currents,” in _Int. Conf. on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 4619–4625.
  * [15] Y. Ma, M. Hu, and X. Yan, “Multi-objective path planning for unmanned surface vehicle with currents effects,” _ISA Transactions_ , vol. 75, pp. 137–156, 2018.
  * [16] A. Mansfield, D. G. Macharet, and M. A. Hsieh, “Energy-efficient orienteering problem in the presence of ocean currents,” in _IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2022, pp. 10 081–10 087.
  * [17] ——, “Energy-efficient team orienteering problem in the presence of time-varying ocean currents,” in _IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2023.
  * [18] M. M. Doshi, M. S. Bhabra, and P. F. Lermusiaux, “Energy–time optimal path planning in dynamic flows: Theory and schemes,” _Computer Methods in Applied Mechanics and Engineering_ , vol. 405, p. 115865, 2023.
  * [19] M. Falcone and R. Ferretti, _Semi-Lagrangian approximation schemes for linear and Hamilton—Jacobi equations_. SIAM, 2013.
  * [20] K. Deb, A. Pratap, S. Agarwal, and T. Meyarivan, “A fast and elitist multiobjective genetic algorithm: Nsga-ii,” _IEEE Trans. on Evolutionary Computation_ , vol. 6, no. 2, pp. 182–197, 2002.
  * [21] H. S. Chang, H.-G. Lee, M. Fu, and S. Marcus, “Evolutionary policy iteration for solving markov decision processes,” _IEEE Trans. on Automatic Control_ , vol. 50, no. 11, pp. 1804–1808, 2005.
  * [22] H. Chang, J. Hu, M. Fu, and S. Marcus, _Simulation-Based Algorithms for Markov Decision Processes_ , ser. Communications and Control Engineering. Springer London, 2013.
  * [23] B. Garau, A. Alvarez, and G. Oliver, “AUV navigation through turbulent ocean environments supported by onboard H-ADCP,” in _IEEE Int. Conf. on Robotics and Automation (ICRA)_ , 2006, pp. 3556–3561.
  * [24] R. Munos and A. Moore, “Variable resolution discretization in optimal control,” _Machine learning_ , vol. 49, pp. 291–323, 2002.
  * [25] G. Barles and P. E. Souganidis, “Convergence of approximation schemes for fully nonlinear second order equations,” _Asymptotic Analysis_ , vol. 4, no. 3, pp. 271–283, 1991.
  * [26] J. Butcher, _Numerical Methods for Ordinary Differential Equations_. Wiley, 2008.
  * [27] D. Cruz-Uribe and C. Neugebauer, “Sharp error bounds for the trapezoidal rule and simpson’s rule.” _J. of Inequalities in Pure & Applied Mathematics_, vol. 3, no. 4, 2002.

|  Victor Costa da Silva Campos is a Professor at the Department of
Electronics Engineering at Universidade Federal de Minas Gerais (UFMG) since
2018. He received a B.Eng. degree in Control and Automation Engineering from
UFMG in 2009, and M.Sc. and Ph.D. degrees in Electrical Engineering also from
UFMG in 2011 and 2015, respectively. His research interests include robust
control, adaptive control, Takagi-Sugeno fuzzy systems, Computational
Intelligence applied to control systems, and motion planning.
---|---
|  Armando Alves Neto received the B.S.E. degree in Automation and Control
Engineering from the Universidade Federal de Minas Gerais in 2006, and S.M.
and Ph.D. degrees in Computer Science from UFMG in 2008 and 2012,
respectively. He is an Assistant Professor at the Department of Electronic
Engineering at UFMG. Research interests include real-time motion planning,
multi-agent control, robust control, and collision avoidance strategies.
---|---
|  Douglas Guimarães Macharet is an Associate Professor at the Dep. of
Computer Science (DCC) of the Universidade Federal de Minas Gerais (UFMG). He
received an M.Sc. and D.Sc. degrees in Computer Science from the same
university in 2009 and 2013, respectively. He is with the Computer Vision and
Robotics Laboratory (VeRLab), and his main research interests are in mobile
robotics, focusing on motion planning and navigation, multi-robot systems,
swarm robotics, and human-robot interaction.
---|---