# Data-Driven Convex Approach to Off-road Navigation via Linear Transfer
Operators

Joseph Moyalan, Yongxin Chen and Umesh Vaidya Financial support from NSF under
grants 1942523, 2008513, 2031573 and NSF CPS award 1932458 is greatly
acknowledged. J. Moyalan and U. Vaidya are with the Department of Mechanical
Engineering, Clemson University, Clemson, SC<EMAIL_ADDRESS>Y. Chen is with the School of Aerospace Engineering, Georgia Institute of
Technology, Atlanta, GA<EMAIL_ADDRESS>
###### Abstract

We consider the problem of optimal navigation control design for navigation on
off-road terrain. We use traversability measure to characterize the degree of
difficulty of navigation on the off-road terrain. The traversability measure
captures the property of terrain essential for navigation, such as elevation
map, terrain roughness, slope, and terrain texture. The terrain with the
presence or absence of obstacles becomes a particular case of the proposed
traversability measure. We provide a convex formulation to the off-road
navigation problem by lifting the problem to the density space using the
linear Perron-Frobenius (P-F) operator. The convex formulation leads to an
infinite-dimensional optimal navigation problem for control synthesis. The
finite-dimensional approximation of the infinite-dimensional convex problem is
constructed using data. We use a computational framework involving the Koopman
operator and the duality between the Koopman and P-F operator for the data-
driven approximation. This makes our proposed approach data-driven and can be
applied in cases where an explicit system model is unavailable. Finally, we
demonstrate the application of the developed framework for the navigation of
vehicle dynamics with Dubin’s car model.

##  I Introduction

The navigation problem is extensively studied in the robotics community. More
recently, the problem of off-road navigation has received more interest and is
driven by robotics applications in an unstructured environment. The objective
is to drive a robot/vehicle from some initial set to reach the desired target
set through an obstacle-ridden environment. One of the popular approaches to
navigation is the use of navigation functions. Controller derived using
navigation functions relies on the negative gradient of the function to drive
the system to the target set. However, due to the nonconvex nature of the
navigation function, the gradient-based controller gets stuck in the local
minimum [1, 2]. Unlike classical navigation problems where the obstacles are
binary, i.e., either there is an obstacle or no obstacle, the entire region
can be riddled with obstacles with varying degrees of difficulty for off-road
navigation. The control design of such navigation problems is based on
geometric mapping of the unstructured environment through the use of sensors
on the mobile robot such as LIDAR and cameras [3, 4, 5]. The control design of
such navigation problems often leads to a nonconvex formulation. Hence
providing guarantees for global optimality remains a challenge. Research
involving vision-based off-road navigation have also gained prominence [6, 7,
8]. However, since the underlying formulation relies on solving a nonconvex
optimization problem, the vision-based approaches lack a systematic structure.

In this paper, we explore the traversability description of the terrain, i.e.,
the level of difficulty in navigating using a traversability measure. The
terrain’s traversability measure depends on the terrain parameters such as
elevation map, terrain roughness, slope, and terrain texture. In this paper,
we have utilized the normalized elevation map while constructing the
traversability measure. The traversability measure is used to assign a varying
degree of difficulty to pass through a given region in the terrain based on
the value of the traversability measure on the region. The larger the value of
traversability measure on a region, the more difficult it is to navigate
through that region. In addition to this, the traversability measure of a
smooth terrain with binary obstacles will be a particular case of the proposed
approach.

We consider the problem of data-driven navigation control design while
considering the traversability constraints. The data-driven element of the
framework makes it possible to work in situations where explicit dynamics of
the vehicle model are not available. For an unmanned ground vehicle (UGV),
this data comes from the wheel odometry, and GPS/IMU sensor [4, 9, 10]. The
main contribution of this paper is in providing a convex formulation to the
optimal navigation problem in off-road terrain. Furthermore, we show how the
traversability measure can be used to design a meaningful cost function
suitable for off-road navigation. The convex formulation is made possible by
lifting the navigation problem in the space of density using methods from
linear operator theory involving Perron-Frobenius operator. The convex
formulation is inspired from the work in [11] for optimal control problems and
later exploited for navigation with deterministic and stochastic obstacles in
[12, 13, 14, 15]. The incorporation of the traversability measure and density-
based formulation for off-road navigation is novel to this paper. The convex
formulation leads to an infinite-dimensional convex optimization problem for
off-road navigation. The finite-dimensional approximation of the infinite-
dimensional convex problem is constructed using data. We use a computational
framework involving the Koopman operator and the duality between the Koopman
and P-F operator for the data-driven approximation. Compared to our earlier
work [11], the data-driven computation framework proposed in this paper is
numerically efficient. Finally, we demonstrate the application of the
developed framework for off-road navigation of vehicle dynamics with the Dubin
car model.

The rest of the paper is structured as follows. Section II provides a brief
introduction to our framework’s necessary preliminaries. Section III consists
of problem formulation and the main results are discussed in Section IV. In
Section V, we develop the computational framework based on the linear operator
framework. Simulation results are presented in Section VI and a conclusion in
Section VII.

##  II Background and Notations

Consider the dynamical system

$\displaystyle\dot{{\mathbf{x}}}={\mathbf{F}}({\mathbf{x}}),\;\;\;{\mathbf{x}}\in{\mathbf{X}}\subseteq{\mathbb{R}}^{n}$
(1)

where it is assumed that
${\mathbf{F}}({\mathbf{x}})\in\mathcal{C}^{1}({\mathbf{X}},{\mathbb{R}}^{n})$,
i.e. the space of continuously differentiable functions on ${\mathbf{X}}$. Let
us denote ${\mathbf{s}}_{t}({\mathbf{x}})$ to be the solution of dynamical
system (1) at time $t$. ${\cal B}({\mathbf{X}})$ denotes the Borel
$\sigma$-algebra on ${\mathbf{X}}$ and ${\cal M}({\mathbf{X}})$ is the vector
space of real-valued measure on ${\cal B}({\mathbf{X}})$. Also note that,
$\mathcal{L}_{\infty}({\mathbf{X}})$ and $\mathcal{L}_{1}({\mathbf{X}})$
represents the space of essentially bounded and integrable functions on
${\mathbf{X}}$ respectively. The lifting of the finite dimensional nonlinear
dynamics from state space to infinite dimension space of functions may be done
in two different ways namely using Koopman and Perron-Frobenius operators
[16].

######  Definition II.1 (Koopman Operator).

$\mathds{U}_{t}$ for dynamical system (1) is defined as

$\displaystyle[\mathds{U}_{t}\phi]({\mathbf{x}})=\phi({\mathbf{s}}_{t}({\mathbf{x}})),$
(2)

where $\phi$ is a test function in the lifted function space
$\mathcal{L}_{\infty}({\mathbf{X}})\bigcap\mathcal{C}^{1}({\mathbf{X}})$. The
infinitesimal generator for the Koopman operator is given by

$\displaystyle\lim_{t\to
0}\frac{[\mathbb{U}_{t}\phi]({\mathbf{x}})-\phi({\mathbf{x}})}{t}={{\mathbf{F}}}({\mathbf{x}})\cdot\nabla\phi({\mathbf{x}})=:{\cal
U}_{{\mathbf{F}}}\phi.$ (3)

######  Definition II.2 (Perron-Frobenius Operator).

$\mathds{P}_{t}:{\cal L}_{1}({\mathbf{X}})\to{\cal L}_{1}({\mathbf{X}})$ for
dynamical system (1) is defined as

$\displaystyle[\mathds{P}_{t}\varphi]({\mathbf{x}})=\varphi({\mathbf{s}}_{-t}({\mathbf{x}}))\left|\frac{\partial{\mathbf{s}}_{-t}({\mathbf{x}})}{\partial{\mathbf{x}}}\right|,$
(4)

where $|.|$ stands for the determinant. The infinitesimal generator for the
P-F operator is given by

$\displaystyle\lim_{t\to
0}\frac{[\mathbb{P}_{t}\varphi]({\mathbf{x}})-\varphi({\mathbf{x}})}{t}=-\nabla\cdot({\mathbf{F}}({\mathbf{x}})\varphi({\mathbf{x}}))=:{\cal
P}_{{\mathbf{F}}}\varphi.$ (5)

These two operators are dual to each other as

$\displaystyle\int_{{\mathbf{X}}}[\mathbb{U}_{t}\phi]({\mathbf{x}})\varphi({\mathbf{x}})d{\mathbf{x}}=\int_{{\mathbf{X}}}[\mathbb{P}_{t}\varphi]({\mathbf{x}})\phi({\mathbf{x}})d{\mathbf{x}}.$
(6)

These two operators enjoy positivity and Markov properties which are used in
the finite dimension approximation of these operators.

######  Property 1.

  1. 1.

Positivity: The P-F and Koopman operators are positive operators i.e., for any
$0\leq\varphi({\mathbf{x}})\in{\cal L}_{\infty}({\mathbf{X}})$ and
$0\leq\psi({\mathbf{x}})\in{\cal L}_{1}({\mathbf{X}})$, we have

$[\mathbb{P}_{t}\psi]({\mathbf{x}})\geq
0,\;\;\;\;[\mathbb{U}_{t}\varphi]({\mathbf{x}})\geq 0,\;\;\;\forall t\geq 0.$
(7)

  2. 2.

Markov Property: The P-F operator satisfies Markov property i.e.,

$\int_{\mathbf{X}}[\mathbb{P}_{t}\psi]({\mathbf{x}})d{\mathbf{x}}=\int_{\mathbf{X}}\psi({\mathbf{x}})d{\mathbf{x}}.$
(8)

######  Definition II.3 (Almost everywhere (a.e.) stability).

The equilibrium point or an attractor set of system (1) is said to be a.e.
uniform stable w.r.t. measure $\mu_{0}\in{\cal M}({\mathbf{X}})$ if

$\displaystyle\mu_{0}\\{{\mathbf{x}}\in{\mathbf{X}}:\lim_{t\to\infty}{\mathbf{s}}_{t}({\mathbf{x}})\neq
0\\}=0,$ (9)

##  III Problem Formulation for Off-road Navigation using Travrersability Map

In this section we give the definition of Traversability map as well as its
mathematical description. We also provide with the choice of cost function for
off-road navigation.

###  III-A Traversability Map

In this paper, we assume that the traversability description of the terrain is
captured using a positive function $b({\mathbf{x}})\in{\cal
L}_{1}({\mathbf{X}})$. We assume that the function $b({\mathbf{x}})$ captures
the information of the elevation map, terrain roughness, slope and terrain
texture. Construction of such map is an active area of interest where on-board
sensors on the vehicles such as vision, LIDAR, and IMU as well as drone
sensory images can be used in the construction of such maps [17, 18, 19].

Now, let $\mu_{b}\in\mathcal{M}({\mathbf{X}})$ be the associated
traversability measure i.e.,
$d\mu_{b}({\mathbf{x}})=b({\mathbf{x}})d{\mathbf{x}}$. For any set
$A\in\mathcal{B}({\mathbf{X}})$, the traversability measure of the set $A$ is
defined using $b({\mathbf{x}})$ as

$\displaystyle{\rm
Trav}(A):=\int_{A}b({\mathbf{x}})d{\mathbf{x}}=:\mu_{b}(A).$ (10)

${\rm Trav}(A)$ captures the relative degree of difficulty of traversing
through the region $A$. In particular, if $\mu_{b}(A_{1})<\mu_{b}(A_{2})$
where $A_{i}\in{\cal B}({\mathbf{X}})$, then the region $A_{2}$ is more
difficult to traverse than region $A_{1}$. It is easy to see that the above
definition of traversability measure captures the information of binary
obstacle as well. In particular, if ${\mathbf{X}}_{u}$ is an obstacle set then
it can be described using the following $b({\mathbf{x}})$

$\displaystyle
b({\mathbf{x}})=\frac{1}{\lambda({\mathbf{X}}_{u})}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}})$
(11)

where $\lambda(\cdot)$ is the Lebesgue measure and
$\mathds{1}_{{\mathbf{X}}_{u}}$ is the indicator function of the set
${\mathbf{X}}_{u}$. To describe the problem formulation for the off-road
navigation, we consider the control dynamical system in the control affine
form as

$\displaystyle\dot{\mathbf{x}}={\bf
f}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{u}}$ (12)

where ${\mathbf{x}}\in{\mathbf{X}}\subset{\mathbb{R}}^{n}$ and
${\mathbf{u}}\in{\mathbf{U}}\subset{\mathbb{R}}^{m}$ are the states and
control input respectively. The control dynamical system is assumed to model
the control dynamics of the vehicle. The control-affine form of the system is
not restrictive and this will typically be the case for the robotics and
vehicle dynamics application [20, 21]. The objective is to determine the
control inputs ${\mathbf{u}}(t)$ to navigate the vehicle dynamics from some
initial state ${\mathbf{X}}_{0}$ to some final state ${\mathbf{X}}_{T}$ while
keeping the traversability cost below some threshold, say $\bar{\gamma}$,
i.e.,

$\displaystyle\int_{0}^{\infty}b({\mathbf{x}}(t))dt\leq\bar{\gamma}$ (13)

where ${\mathbf{x}}(t)$ is the trajecotry of the control system (17). In this
paper, we are interested in asymptotic naviagation problem. In the following
subsection, we will discuss possible choices of cost function that are
meaningful for the off-road navigation problem.

###  III-B Choice of Cost function for Off-road Navigation

One of the obvious approach for constructing the cost function for off-road
navigation is to minimize the traversability cost i.e.,

$\displaystyle\min_{{\mathbf{u}}}\;\int_{0}^{\infty}b({\mathbf{x}}(t))dt.$
(14)

At first glance it appears that the form of cost function will incentives
traversing through the region where the traversability function
$b({\mathbf{x}})$ takes smaller values. However, with no cost on the control
input it is possible for the system trajectories to pass through the region
with large value of $b({\mathbf{x}})$ and hence deemed unfavorable to traverse
at very high speed without negatively impacting the cost. Clearly, this kind
of behavior is not tolerable. To address this problem, the cost function can
be modified to ensure that occupancy in the nontravesable region is minimized
as follows

$\displaystyle\min_{{\mathbf{u}}}V_{1}({\mathbf{x}})=\min_{{\mathbf{u}}}\;\int_{0}^{\infty}b({\mathbf{x}}(t))\|{\mathbf{u}}(t)\|_{1}dt.$
(15)

where $\|\cdot\|_{1}$ is the vector one norm. The above form of cost function
will penalize the control input and the hence the speed of the vehicle in the
nontraversable region. Another modification to the cost function can be
considered as follows:

$\displaystyle\min_{{\mathbf{u}}}\;V_{2}({\mathbf{x}})=\min_{{\mathbf{u}}}\;\int_{0}^{\infty}\alpha
b({\mathbf{x}}(t))+\beta b({\mathbf{x}}(t))\|{\mathbf{u}}(t)\|_{1}dt.$ (16)

where $\alpha$ and $\beta$ are positive parameters and determine the relative
weights of the traversability measure $b({\mathbf{x}})$ and traverability
times speed i.e, $b({\mathbf{x}})|{\mathbf{u}}|$.

Our proposed density-based formulation relies on minimization of the cost
function which is average or expected value of the above defined cost
functions (31)-(16). The expected value is taken with respect to the initial
measure $\mu_{0}$ capturing the distribution of the initial state. Under the
assumption that $\mu_{0}$ is supported on the initial set ${\mathbf{X}}_{0}$,
we consider following cost function in our density-based formulation

$\displaystyle
J_{k}(\mu_{0})=\int_{{\mathbf{X}}}V_{k}({\mathbf{x}})d\mu_{0}({\mathbf{x}}),\;\;k=1,2$
(17)

where $\mu_{0}$ is the given initial measure assumed to absolutely continuous
with density function $h_{0}$ i.e.,
$\frac{\partial\mu_{0}}{\partial{\mathbf{x}}}=h_{0}({\mathbf{x}})$.
Appropriate conditions on the initial measure $\mu_{0}$ are necessary to
ensure that the cost function is finite. We will discuss this in the main
results section. Along with minimizing the cost function it is also of
interest to avoid certain obstacle set, ${\mathbf{X}}_{u}$, and limit the
control authority i.e., $|u_{j}|\leq L_{j}$. The obstacle avoidance
constraints for a.e trajectories starting from the initial set
${\mathbf{X}}_{0}$ can be written as

$\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}}(t))d\mu_{0}({\mathbf{x}})=0,\;\;\forall
t\geq 0.$

where ${\mathbf{x}}(t)$ is the solution of system (12) starting from initial
condition ${\mathbf{x}}$. With the above definition, the problem statement for
the optimal off-road naviagation using the traversability map can be stated as
follows.

######  Problem 1.

(Optimal off-road Navigation Problem): The optimal off-road navigation problem
consists of navigation almost every system trajectories for the control system
(12) starting from the initial set ${\mathbf{X}}_{0}$ to the target set
${\mathbf{X}}_{T}$ while avoiding the obstacle set ${\mathbf{X}}_{u}$ such
that the following cost function is minimized and cost for traversing is kept
below some threshold. Mathematically, it can be written as

$\displaystyle\min_{{\mathbf{u}}}$
$\displaystyle\;\;\;\int_{{\mathbf{X}}}\int_{0}^{\infty}\alpha
b({\mathbf{x}}(t))+\beta
b({\mathbf{x}}(t))\|{\mathbf{u}}(t)\|_{1}dtd\mu_{0}({\mathbf{x}})$ (18a)
$\displaystyle{\rm s.t.}$
$\displaystyle\;\;\;\;\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}}(t))d\mu_{0}({\mathbf{x}})=0,\;\;\forall
t\geq 0$ (18b)
$\displaystyle\int_{{\mathbf{X}}}\int_{0}^{\infty}b({\mathbf{x}}(t))dtd\mu_{0}({\mathbf{x}})\leq\gamma$
(18c) $\displaystyle|u_{j}|\leq L_{j}$ (18d)
$\displaystyle\dot{\mathbf{x}}={\bf
f}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{u}},\;\;\;\;\lim_{t\to\infty}{\mathbf{x}}(t)\in{\mathbf{X}}_{T}$
(18e)

above has to be true for almost all ${\mathbf{x}}(0)\in{\mathbf{X}}_{0}$. In
the following section, we prove that the optimal off-road navigation problem
can be written as a convex optimization problem over the density space.

##  IV Convex Formulation to Optimal off-Road Navigation

We consider the control affine dynamical system

$\displaystyle\dot{\mathbf{x}}={\bf
f}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{u}}$ (19)

where ${\bf f}({\mathbf{x}})\in{\cal C}^{1}({\mathbf{X}},{\mathbb{R}}^{n})$
and ${\bf g}\in{\cal C}^{1}({\mathbf{X}},{\mathbb{R}}^{m})$. Following
assumption is made on the control dynamical system and the controller for the
system (19).

######  Assumption 1.

For the optimal off-road navigation problem we assume that the optimal control
input is feedback in nature i.e.,
${\mathbf{u}}={\mathbf{k}}({\mathbf{x}})\in{\cal C}^{1}({\mathbf{X}})$, such
that the cost function corresponding to this input is finite.

With the above assumption we can write the feedback control system in the form

$\displaystyle\dot{\mathbf{x}}={\bf
f}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{k}}({\mathbf{x}})=:{\mathbf{F}}_{c}({\mathbf{x}}).$
(20)

We will use the notations ${\mathbf{s}}^{c}_{t}({\mathbf{x}})$,
${\mathbb{U}}_{t}^{c}$, and ${\mathbb{P}}_{t}^{c}$ to denotes the solution,
the Koopman operator, and the P-F operator for the closed loop system (20).
The first results of this section allows us to write the obstacle avoidance
constraints (18b) in the integral form.

######  Lemma 1.

For the dynamical system (20), if

$\displaystyle\int_{0}^{\infty}\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{s}}_{t}^{c}({\mathbf{x}}))d\mu_{0}({\mathbf{x}})dt=0$
(21)

then

$\displaystyle\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{s}}_{t}^{c}({\mathbf{x}}))h_{0}({\mathbf{x}})d{\mathbf{x}}=0,\;\;\;\forall
t\geq 0,$ (22)

i.e., the amount of time system trajectories spend in the region
${\mathbf{X}}_{u}$ starting from the positive measure set of initial condition
corresponding to the initial set, ${\mathbf{X}}_{0}$, with density
$h_{0}({\mathbf{x}})$ is equal to zero.

###### Proof.

Proof by contradiction. Assume (22) is not true, i.e., there exists some time
$t_{0}$ for which

$\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{s}}_{t_{0}}^{c}({\mathbf{x}}))h_{0}({\mathbf{x}})d{\mathbf{x}}=\int_{{\mathbf{X}}_{1}}[\mathbb{U}_{t_{0}}^{c}\mathds{1}_{{\mathbf{X}}_{u}}]({\mathbf{x}})h_{0}({\mathbf{x}})d{\mathbf{x}}>0$

Then using the continuity property of the Koopman semi-group, we know there
exists a $\Delta$ such that

$\int_{t_{0}}^{t_{0}+\Delta}\int_{{\mathbf{X}}}[\mathbb{U}_{t_{0}}^{c}\mathds{1}_{{\mathbf{X}}_{u}}](({\mathbf{x}}))h_{0}({\mathbf{x}})d{\mathbf{x}}dt>0.$

We have

$\displaystyle
0<\int_{t_{0}}^{t_{0}+\Delta}\int_{{\mathbf{X}}}[\mathbb{U}_{t_{0}}^{c}\mathds{1}_{{\mathbf{X}}_{u}}]({\mathbf{x}})h_{0}({\mathbf{x}})d{\mathbf{x}}dt\leq$
(23)
$\displaystyle\int_{0}^{\infty}\int_{{\mathbf{X}}}[\mathbb{U}_{t_{0}}^{c}\mathds{1}_{{\mathbf{X}}_{u}}]({\mathbf{x}})h_{0}({\mathbf{x}})d{\mathbf{x}}dt=0$

∎

######  Remark.

Following the results of Lemma 1, we can replace the the obstacle avoidance
constraints (18b) as

$\displaystyle\int_{0}^{\infty}\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{s}}_{t}^{c}({\mathbf{x}}))d\mu_{0}({\mathbf{x}})dt=0$
(24)

######  Remark.

In the rest of the paper, we will use notation
${\mathbf{X}}_{1}={\mathbf{X}}\setminus{\cal N}_{\epsilon}$, where ${\cal
N}_{\epsilon}$ is the $\epsilon$ neighborhood of the target set. With no loss
of generality we can assume that the target set ${\mathbf{X}}_{T}$ is locally
stable with domain of attraction containing ${\cal N}_{\epsilon}$.

We are now ready to state the main results of this paper.

######  Theorem 2.

Under Assumption 1, the optimal off-road navigation problem can be written as
following convex optimization problem in term of optimization variable
$\rho\in{\cal L}_{1}({\mathbf{X}}_{1})\cap{\cal
C}^{1}({\mathbf{X}}_{1},{\mathbb{R}}_{\geq 0})$ and
$\bar{\boldsymbol{\rho}}\in{\cal C}^{1}({\mathbf{X}}_{1},{\mathbb{R}})$.

$\displaystyle J^{\star}(\mu)=$
$\displaystyle\inf_{\rho,\bar{\boldsymbol{\rho}}}\int_{{\mathbf{X}}_{1}}\alpha
b({\mathbf{x}})\rho({\mathbf{x}})+\beta
b({\mathbf{x}})\;\|\bar{{\boldsymbol{\rho}}}({\mathbf{x}})\|_{1}\;d{\mathbf{x}}$
(25a) $\displaystyle{\rm s.t}.\;\;\;$
$\displaystyle\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}=0$
(25b)
$\displaystyle\int_{{\mathbf{X}}_{1}}b({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}\leq\gamma$
(25c) $\displaystyle|\bar{\rho}_{j}({\mathbf{x}})|\leq
L_{j}\rho({\mathbf{x}})$ (25d)
$\displaystyle\nabla\cdot({\mathbf{f}}\rho+{\mathbf{g}}\bar{\boldsymbol{\rho}})=h_{0},\;\;{\rm
a.e.}\;{\mathbf{x}}\in{\mathbf{X}}_{1}$ (25e)

The optimal feedback control input is recovered from the solution of the above
optimization problem as

$\displaystyle
k^{\star}({\mathbf{x}})=\frac{\bar{{\boldsymbol{\rho}}}^{\star}({\mathbf{x}})}{\rho^{\star}({\mathbf{x}})}$
(26)

where $(\rho^{\star},\bar{{\boldsymbol{\rho}}}^{\star})$ are the solution of
(25).

###### Proof.

Following Assumption 1, we write the feedback control system

$\displaystyle\dot{{\mathbf{x}}}={\mathbf{f}}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{k}}({\mathbf{x}}).$
(27)

Let $\mathds{P}^{c}_{t}$ and $\mathds{U}^{c}_{t}$ be P-F and Koopman operator
for the feedback control system (27). Now we use the fact that
$d\mu_{0}=h_{0}({\mathbf{x}})d{\mathbf{x}}$, and the definition of Koopman
operator to write the cost function (18a) as

$\displaystyle
J(\mu_{0})=\int_{{\mathbf{X}}_{1}}\int^{\infty}_{0}[\mathds{U}^{c}_{t}(\alpha
b+\beta
b\|{\mathbf{k}}\|_{1})]({\mathbf{x}})\;dth_{0}({\mathbf{x}})d{\mathbf{x}}$
$\displaystyle=\int_{{\mathbf{X}}_{1}}\int^{\infty}_{0}(\alpha b+\beta
b\|{\mathbf{k}}\|_{1})({\mathbf{s}}_{t}^{c}({\mathbf{x}}))\;dth_{0}({\mathbf{x}})d{\mathbf{x}}$

Performing change of variable
${\mathbf{y}}={\mathbf{s}}_{t}^{c}({\mathbf{x}})$ or
${\mathbf{x}}={\mathbf{s}}_{-t}^{c}({\mathbf{y}})$ and using the definition of
P-F operator we can write the above as

$\displaystyle J(\mu_{0})$
$\displaystyle=\int_{{\mathbf{X}}_{1}}\int^{\infty}_{0}(\alpha b+\beta
b\|{\mathbf{k}}\|_{1})({\mathbf{x}})[\mathds{P}^{c}_{t}h_{0}]\;dtd{\mathbf{x}}$
$\displaystyle=\int_{{\mathbf{X}}_{1}}(\alpha b+\beta
b\|{\mathbf{k}}\|_{1})({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}$ (28)

where

$\displaystyle\rho({\mathbf{x}}):=\int_{0}^{\infty}[{\mathbb{P}}^{c}_{t}h_{0}]({\mathbf{x}})dt.$
(29)

Since the cost function is assumed to be finite (Assumption 1), the
$\rho({\mathbf{x}})$ is well-defined for a.e. ${\mathbf{x}}$ and is an
integrable function. From the definition of the P-F operator and the
assumption made on function $h_{0}$ it follows that the function
$[{\mathbb{P}}_{t}^{c}h_{0}]({\mathbf{x}})$ is uniformly continuous function
of time. Hence using Barbalat Lemma [22, pg. 269] we have

$\displaystyle\lim_{t\to\infty}[{\mathbb{P}}_{t}h_{0}]({\mathbf{x}})=0,$ (30)

for a.e. ${\mathbf{x}}$. Furthermore, since the P-F operator is a positive
operator (Proeprty 1) and $h_{0}({\mathbf{x}})\geq 0$, we have
$\rho({\mathbf{x}})\geq 0$. Hence (31) can be written as

$\displaystyle J(\mu_{0})$ $\displaystyle=\int_{{\mathbf{X}}_{1}}\alpha
b({\mathbf{x}})\rho({\mathbf{x}})+\beta
b({\mathbf{x}})\|\bar{\boldsymbol{\rho}}({\mathbf{x}})\|_{1})d{\mathbf{x}}$
(31)

where
$\bar{\boldsymbol{\rho}}({\mathbf{x}}):=\rho({\mathbf{x}}){\mathbf{k}}({\mathbf{x}})$.
This shows that the cost function in (18) can be written in the form (25). We
next discuss the constraints. Following the results of Lemma 1, we know that
(21) implies (22). Hence, the obstacle avoidance constraints (18c) is implied
by

$\displaystyle\int_{0}^{\infty}\int_{{\mathbf{X}}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{s}}_{t}^{c}({\mathbf{x}}))d\mu_{0}({\mathbf{x}})dt=0$
(32)

Again performing change of variables
${\mathbf{y}}={\mathbf{s}}_{t}^{c}({\mathbf{x}})$ and using the definition of
P-F operator, (32) can be written as

$\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}=0$

with $\rho({\mathbf{x}})$ as defined in (29). Similarly, it follows that (18d)
can be written as

$\int_{{\mathbf{X}}_{1}}b({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}\leq\gamma.$

The control constraints $|u_{j}|\leq L_{j}$ for $j=1,\ldots,m$ can be written
as $|\bar{\rho}_{j}({\mathbf{x}})|\leq L_{j}\rho({\mathbf{x}})$ follows from
the fact that
${\mathbf{u}}={\mathbf{k}}({\mathbf{x}})=\bar{\boldsymbol{\rho}}({\mathbf{x}})/\rho({\mathbf{x}})$
and $\rho({\mathbf{x}})$ is positive. We next show that the
$\rho({\mathbf{x}})$ as defined in (29) satisfies the constraints (25e).
Substituting (29) in the constraint of (25e), we obtain

$\displaystyle\nabla\cdot({\bf
f}_{c}({\mathbf{x}})\rho({\mathbf{x}}))=\int_{0}^{\infty}\nabla\cdot({\bf
f}_{c}({\mathbf{x}})[\mathbb{P}_{t}^{c}h_{0}]({\mathbf{x}}))dt$
$\displaystyle=$
$\displaystyle\int_{0}^{\infty}-\frac{d}{dt}[\mathbb{P}^{c}_{t}h_{0}]({\mathbf{x}})dt=-[\mathbb{P}^{c}_{t}h_{0}]({\mathbf{x}})\Big{|}^{\infty}_{t=0}=h_{0}({\mathbf{x}}),$

where, ${\bf f}_{c}({\mathbf{x}}):={\bf
f}({\mathbf{x}})+{\mathbf{g}}({\mathbf{x}}){\mathbf{k}}({\mathbf{x}})$. In
deriving (LABEL:eq11) we have used the infinitesimal generator property of P-F
operator Eq. (5) and the fact that
$\lim_{t\to\infty}[\mathbb{P}_{t}^{c}h_{0}]({\mathbf{x}})=0$ following (30).
We next show that the target set ${\mathbf{X}}_{T}$ is a.e. stable w.r.t
measure $\mu_{0}$ supported on the initial set ${\mathbf{X}}_{0}$. Consider
the set $S_{\ell}$

$S_{\ell}=\\{{\mathbf{x}}\in{\mathbf{X}}_{1}:{\mathbf{s}}^{c}_{t}({\mathbf{x}})\in{\mathbf{X}}_{1},\;{\rm
for\;some}\;t>\ell\\}$

and let

$S=\bigcap_{\ell=1}^{\infty}S_{\ell}$

The set $S$ contains points some of whose limit points lie in
${\mathbf{X}}_{1}$ and for almost every stability of the target set we need to
show that $\mu_{0}(S)=0$. From the construction of the set $S$, it follows
that ${\mathbf{s}}_{t}^{c}(S)=S$, where
${\mathbf{s}}_{t}^{c}(S)=\\{{\mathbf{s}}_{t}^{c}({\mathbf{x}}):{\mathbf{x}}\in
S\\}$.

$\displaystyle\mu_{0}(S)=\int_{{\mathbf{X}}_{1}}\lambda_{S}({\mathbf{x}})h_{0}({\mathbf{x}})d{\mathbf{x}}=\int_{{\mathbf{X}}_{1}}\lambda_{S}({\mathbf{s}}_{t}^{c}({\mathbf{x}}))h_{0}({\mathbf{x}})d{\mathbf{x}}$
$\displaystyle\int_{{\mathbf{X}}_{1}}\lambda_{S}({\mathbf{x}})[{\mathbb{P}}_{t}h_{0}]({\mathbf{x}})d{\mathbf{x}}$
(34)

where we have use the fact that ${\mathbf{x}}\in S$ iff
${\mathbf{s}}_{t}^{c}({\mathbf{x}})\in S$. Since the above is true for all
$t\geq 0$, we obtain using dominated convergence theorem

$\mu_{0}(S)=\lim_{t\to\infty}\int_{S}[{\mathbb{P}}_{t}h_{0}]({\mathbf{x}})d{\mathbf{x}}=\int_{S}\lim_{t\to\infty}[{\mathbb{P}}_{t}h_{0}]({\mathbf{x}})d{\mathbf{x}}=0$

∎

Other constraints on the state and control input that can be written convexly
in terms of the optimization variables $\rho$ and $\bar{\boldsymbol{\rho}}$
can be incorporated in the framework. As we show in the simulation section one
of such constraints is the curvature constraints. For example, consider a
Dubin’s car model

$\dot{x}_{1}=u_{1}\cos\theta,\;\;\dot{x}_{2}=u_{1}\sin\theta,\;\;\dot{\theta}=u_{2}.$

Aside from the kinematic constraints imposed by nonholonomy, most often the
additional constraint that the radius of curvature of the paths of the vehicle
are lower bounded must be considered [23]. The curvature constraints itself
will be function of the terrain properties. The curvature constraints makes
the kinematic model of Dubin’s car more close to reality. The curvature
constraints are captured as follows.

$\displaystyle\frac{|u_{2}|}{|u_{1}|}\leq\frac{1}{\mathds{C}}$ (35)

where $u_{2}$ is the angular velocity of the Dubin’s car model. The constraint
in (35) makes sure that control design are realistic and that angular velocity
of the kinematic model is bounded. The (35) can be easily added to the
optimization problem of theorem 2 convexly as follows. Following the general
formulation specialize for two input system, we can write
$u_{1}=\frac{\bar{\rho}_{1}}{\rho}$ and $u_{2}=\frac{\bar{\rho}_{2}}{\rho}$,
hence

$\displaystyle\frac{|u_{2}|}{|u_{1}|}\leq\frac{1}{\mathds{C}}\implies\mathds{C}|\bar{\rho}_{2}|-|\bar{\rho}_{1}|\leq
0$ (36)

which is linear and hence convex in $\bar{\rho}_{1}$ and $\bar{\rho}_{2}$.

##  V Computational Framework

In this section, we provide with a new data driven approximation, namely
NSDMD*, which is a modification of Naturally structured dynamic mode
decomposition (NS- DMD) [24]. Then we will formulate our problem statement
with the given compuatational framework.

###  V-A Data-Driven Approximation: NSDMD*

Naturally structured dynamic mode decomposition (NSDMD) is a modification of
Extended Dynamic Mode Decomposition (EDMD) algorithm [25], one of the popular
algorithms used for the data-driven approximation of the Koopman operator. The
modifications in NSDMD were introduced to incorporate the natural properties
of these operators namely the positivity and the Markov properties. However,
these modifications are computationally expensive. Our proposed transfer
operator theoretic framework for the control design relies on the positivity
and Markov properties of these operators. In our proposed modification of the
NSDMD algorithm we gain numerical efficiency at the expense of preserving
these properties approximately. We call this modified algorithm as NSDMD⋆. For
the continuous-time dynamical system (1), consider snapshots of data set
obtained as time-series data from single or multiple trajectories.

$\displaystyle{\mathcal{X}}=[{\mathbf{x}}_{1},{\mathbf{x}}_{2},\ldots,{\mathbf{x}}_{M}],\;\;\;\;{\cal
Y}=[\mathbf{y}_{1},\mathbf{y}_{2},\ldots,\mathbf{y}_{M}],$ (37)

where ${\mathbf{x}}_{i}\in{\mathbf{X}}$ and $\mathbf{y}_{i}\in{\mathbf{X}}$.
The pair of data sets are assumed to be two consecutive snapshots i.e.,
$\mathbf{y}_{i}={\mathbf{s}}_{\Delta t}({\mathbf{x}}_{i})$. In our proposed
data-driven computation, the pair of two consecutive snapshots are obtained
using system with no control input $({\mathbf{s}}^{0}_{\Delta
t}({\mathbf{x}}_{i}))$ and with the step input $({\mathbf{s}}^{1}_{\Delta
t}({\mathbf{x}}_{i}))$. Let
${{\mathbf{\Psi}}}=[\psi_{1},\ldots,\psi_{N}]^{\top}$ be the choice of basis
functions. The EDMD algorithm computes the finite-dimensional approximation of
the Koopman operator ($\bf U$) as the result of the following least square
problem.

$\min\limits_{\bf U}\parallel{\bf G}{\bf U}-{\bf A}\parallel_{F},$ (38)

where,

$\displaystyle{\bf
G}=\frac{1}{M}\sum_{m=1}^{M}{\mathbf{\Psi}}({{\mathbf{x}}}_{m}){\mathbf{\Psi}}({{\mathbf{x}}}_{m})^{\top},$
(39) $\displaystyle{\bf
A}=\frac{1}{M}\sum_{m=1}^{M}{\mathbf{\Psi}}({{\mathbf{x}}}_{m}){\mathbf{\Psi}}({\mathbf{y}}_{m})^{\top},$
(40)

with ${\bf U},{\bf G},{\bf A}\in\mathbb{R}^{N\times N}$, $\|\cdot\|_{F}$
stands for Frobenius norm. The above least square problem admits an analytical
solution

$\displaystyle{\bf U}^{\star}=\bf{G}^{\dagger}\bf{A}.$ (41)

Convergence results for EDMD algorithms in the limit as the number of data
points and basis functions go to infinity are provided in [26, 27]. In this
paper, we work with Gaussian Radial Basis Function (RBF) for the finite-
dimensional approximation of the linear operators, which are positive basis.
Under the assumption that the basis functions are positive our proposed NSDMD⋆
obtains $\hat{\mathbf{U}}$ from ${\mathbf{U}}^{\star}$ using row normalization
i.e., the entries of the matrix $\hat{\mathbf{U}}$ are obtained as follows.

$\displaystyle[\hat{{\mathbf{U}}}]_{ij}=\frac{[{\mathbf{U}}^{\star}]_{ij}}{\sum_{j}[{\mathbf{U}}^{\star}]_{ij}}$
(42)

The P-F matrix is then obtained as ${\hat{\bf P}}=\hat{\bf U}^{\top}$. The
generator of P-F operator are obtained as

$\displaystyle{\cal P}_{{\mathbf{F}}}\approx\frac{\hat{\bf P}-{\bf I}}{\Delta
t}=:{\mathbf{M}}.$ (43)

Note that in the above construction, we did not strictly enforce the
positivity property on the linear operators. However, the numerical evidence
suggest that the entries of the matrix, ${\mathbf{U}}^{\star}$ obtained using
the EDMD algorithm with positive basis function are predominantly positive.

###  V-B Problem Statement Computational Framework

This section discusses about finite dimensional approximation of the infinite
dimensional optimal navigation problem (25a)-(25c). We use the approximation
of the generator for the vector field ${\mathbf{f}}$ and ${\mathbf{g}}$. The
generator approximation of the two vector fields are given as follows

$\displaystyle\mathcal{P}_{{\mathbf{f}}}\approx\textbf{M}_{0},\;\;\;\;\mathcal{P}_{{\mathbf{g}}}\approx\textbf{M}_{1}$
(44)

The generator approximation of $\mathcal{P}_{{\mathbf{f}}}$ starts with
obtaining open-loop time series data
$\\{{\mathbf{x}}_{k}^{0}\\}=\\{{\mathbf{x}}_{0}^{0},{\mathbf{x}}_{1}^{0},\dots,{\mathbf{x}}_{M}^{0}\\}$
by substituting ${\mathbf{u}}=0$ in (12). Similarly, the approximation of
$\mathcal{P}_{{\mathbf{g}}_{i}}$ is obtained by getting
$\\{{\mathbf{x}}_{k}^{i}\\}=\\{{\mathbf{x}}_{0}^{i},{\mathbf{x}}_{1}^{i},\dots,{\mathbf{x}}_{M}^{i}\\}$
from substituting ${\mathbf{u}}={\mathbf{u}}_{i}$ in (12) where
${\mathbf{u}}_{i}$ is a column vector consisting of all zeros except at
$i^{th}$ position where the element value is 1. Then we calculate the matrices
$\hat{{\mathbf{U}}}^{0}$ and $\hat{{\mathbf{U}}}^{i}$ by utilizing (38)-(42).
The P-F matrix is then obtained as
$\hat{{\mathbf{P}}}^{0}=\hat{{\mathbf{U}}}^{0\top}$ and
$\hat{{\mathbf{P}}}^{i}=\hat{{\mathbf{U}}}^{i\top}$. Then we calculate
$\textbf{M}_{0}$ and $\textbf{M}_{i}$ as follows:

$\displaystyle{\cal P}_{{\mathbf{f}}}\approx\frac{\hat{\bf P}^{0}-{\bf
I}}{\Delta t}=:{\mathbf{M}}_{0},\;\;{\cal
P}_{\textbf{f+g${}_{i}$}}\approx\frac{\hat{\bf P}^{i}-{\bf I}}{\Delta t}$ (45)
$\displaystyle{\rm and}\;\;{\cal P}_{\textbf{g${}_{i}$}}={\cal
P}_{\textbf{f+g${}_{i}$}}-{\cal P}_{{\mathbf{f}}}=:{\mathbf{M}}_{i}$ (46)

NSDMD* algorithm outlined in Section V-A is used to approximate with
${\mathbf{\Psi}}({\mathbf{x}})=[\psi_{1}({\mathbf{x}}),\psi_{2}({\mathbf{x}}),\dots,\psi_{N}({\mathbf{x}})]^{\top}$
as the basis functions. Let
$\bar{{\boldsymbol{\rho}}}({\mathbf{x}})=[\bar{\rho}_{1}({\mathbf{x}}),\dots,\bar{\rho}_{m}({\mathbf{x}})]^{\top}$
and
$\mathbf{\Gamma}({\mathbf{x}})=[\Gamma_{1}({\mathbf{x}}),\dots,\Gamma_{m}({\mathbf{x}})]^{\top}$.
Now let us express the following terms as combinations of basis functions

$\displaystyle h_{0}({\mathbf{x}})={\mathbf{\Psi}}^{\top}{\bf
m},\;\rho({\mathbf{x}})={\mathbf{\Psi}}^{\top}{\bf
v},\;{\bar{\rho}}_{j}({\mathbf{x}})={\mathbf{\Psi}}^{\top}{\bf w}_{j},\;$
$\displaystyle\Gamma_{j}({\mathbf{x}})={\mathbf{\Psi}}^{\top}\textbf{r}_{j}.$
(47)

where $|\bar{\rho}_{j}({\mathbf{x}})|<\Gamma_{j}({\mathbf{x}})$. Minimizing
$\Gamma_{j}({\mathbf{x}})$ will indirectly minimize
$|\bar{\rho}_{j}({\mathbf{x}})|$. Therefore we replace
$\|\bar{{\boldsymbol{\rho}}}({\mathbf{x}})\|_{1}$ by
$\sum_{j}\Gamma_{j}({\mathbf{x}})$ in (25a) and then perform the following
finite-dimensional approximation.

$\displaystyle\inf_{\rho,\bar{\boldsymbol{\rho}}}$
$\displaystyle\int_{{\mathbf{X}}_{1}}\left[\alpha
b({\mathbf{x}})\rho({\mathbf{x}})+\beta
b({\mathbf{x}})\;\sum_{j}\Gamma_{j}({\mathbf{x}})\;\right]d{\mathbf{x}}\approx$
$\displaystyle\underset{\textbf{r}_{j},{\bf w}_{j},{\bf v}}{\min}\;{\bf
D_{1}}^{\top}{\bf v}+{\bf D_{2}}^{\top}\sum_{j}\textbf{r}_{j}$ (48a)
$\displaystyle\text{s.t.}~{}~{}\textbf{r}_{j}-{\bf w}_{j}>\mathbf{0}$ (48b)
$\displaystyle\quad\quad\textbf{r}_{j}+{\bf w}_{j}>\mathbf{0}$ (48c)

where ${\bf D}_{1}=\int_{{\mathbf{X}}_{1}}\alpha
b({\mathbf{x}})\;\Psi({\mathbf{x}})\;d{\mathbf{x}}$ , ${\bf
D}_{2}=\int_{{\mathbf{X}}_{1}}\beta
b({\mathbf{x}})\;\Psi({\mathbf{x}})\;d{\mathbf{x}}$ and $\mathbf{0}$ is a zero
vector.

Similarly, we can write

$\displaystyle\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}\approx\left[\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}}){\mathbf{\Psi}}({\mathbf{x}})d{\mathbf{x}}\right]\bf
v$ $\displaystyle={\mathbf{d_{1}}}^{\top}\bf v$ (49)

where
${\mathbf{d_{1}}}:=\int_{{\mathbf{X}}_{1}}\mathds{1}_{{\mathbf{X}}_{u}}({\mathbf{x}}){\mathbf{\Psi}}({\mathbf{x}})d{\mathbf{x}}$.

Similarly,

$\displaystyle\int_{{\mathbf{X}}_{1}}b({\mathbf{x}})\rho({\mathbf{x}})d{\mathbf{x}}\approx\left[\int_{{\mathbf{X}}_{1}}b({\mathbf{x}}){\mathbf{\Psi}}^{\top}({\mathbf{x}})d{\mathbf{x}}\right]\bf
v$ $\displaystyle={\mathbf{d_{2}}}^{\top}\bf v$ (50)

where
${\mathbf{d_{2}}}:=\int_{{\mathbf{X}}_{1}}b({\mathbf{x}}){\mathbf{\Psi}}({\mathbf{x}})d{\mathbf{x}}$.

######  Assumption 2.

We consider all the basis functions to be positive and linearly independent.

######  Remark.

We use Gaussian Radial Basis Functions (RBF) in this paper to obtain all the
simulation results i.e.,
$\psi_{k}({\mathbf{x}})=\exp^{-\frac{\parallel{\mathbf{x}}-{\bf
c}_{k}\parallel^{2}}{2\sigma^{2}}}$ where ${\mathbf{c}}_{k}$ is the center of
the $k^{th}$ Gaussian RBF.

Therefore the ONP in Theorem 2 can be written as

$\displaystyle\underset{\textbf{r}_{j},{\bf w}_{j},{\bf v}}{\min}\;{\bf
D_{1}}^{\top}{\bf v}+{\bf D_{2}}^{\top}\sum_{j}\textbf{r}_{j}$ s.t.
$\displaystyle\textbf{r}_{j}-{\bf w}_{j}>\mathbf{0}$
$\displaystyle\textbf{r}_{j}+{\bf w}_{j}>\mathbf{0}$
$\displaystyle-{\mathbf{M}}_{0}{\bf v}-\sum_{j}{\mathbf{M}}_{j}{\bf w}_{j}=\bf
m$ $\displaystyle\mathbf{d_{1}}{\bf v}=0$ $\displaystyle\mathbf{d_{2}}{\bf
v}\leq\gamma$ $\displaystyle{|{\bf w}_{j}|}\leq L_{j}\bf v$

##  VI Simulation Results

The simulation results in this paper are obtained using Gaussian RBF. The
centers of the RBF are chosen to be uniformly distributed in the state space
at a distance of $d$. The standard deviation $\sigma$ for the Gaussian RBF is
chosen to be between $d\leq 3\sigma\leq 1.5d$. All the simulations results are
performed using MATLAB on a desktop computer with 64 GB RAM. The linear
approximation of the underlying system dynamics are computated using the data
of the UGV. In this paper, we use the dubin’s car model dynamics to present
the results. CVX toolbox is used for solving optimization problem.

Example. The simulation results are obtained using Dubin’s car dynamics

$\displaystyle\dot{x}_{1}=u_{1}\;\cos\theta,\;\;\;\dot{x}_{2}=u_{1}\;\sin\theta\;\;\;\dot{\theta}=u_{2}$
(51)

We consider two different configuration of initial and terminal set labeled as
${\mathbf{X}}_{01},{\mathbf{X}}_{02}$ and
${\mathbf{X}}_{T1},{\mathbf{X}}_{T2}$. These sets are described as follows.

  * •

${\mathbf{X}}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{3}:-3\leq x_{1}\leq
9,\;-3\leq x_{2}\leq 9,\;-3\leq x_{3}\leq 3\\}$

  * •

${\mathbf{X}}_{01}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{2}:1-(x_{1}-5.5)^{2}-(x_{2}-7.5)^{2}\geq
0\\}$

  * •

${\mathbf{X}}_{02}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{2}:1-(x_{1}+0.5)^{2}-(x_{2}+1.5)^{2}\geq
0\\}$

  * •

${\mathbf{X}}_{T1}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{2}:1-x_{1}^{2}-x_{2}^{2}\geq
0\\}$

  * •

${\mathbf{X}}_{T2}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{2}:1-(x_{1}-8)^{2}-(x_{2}-3)^{2}\geq
0\\}$

  * •

${\mathbf{X}}_{u}\triangleq\\{{\mathbf{x}}\in{\mathbb{R}}^{2}:1-(x_{1}-3)^{2}-(x_{2}-4)^{2}\geq
0\\}$

The linear operators are approximated using $2500$ RBF and the time-step of
discretization $\Delta t=0.01$. We have obtained the results for two different
traversability maps labeled by $b_{1}({\mathbf{x}})$ and
$b_{2}({\mathbf{x}})$. In Fig. 1, we show the plot for initial conditions
centered around $(6.4,8.5)$ in initial set ${\mathbf{X}}_{01}$ navigating
successfully to terminal set ${\mathbf{X}}_{T1}$ with traversability map given
by $b_{1}({\mathbf{x}})$. We notice that the trajectories navigate through a
region where the traversability map takes smaller values. Similarly, Fig. 2
shows the simulation results for optimal navigation from the initial set
${\mathbf{X}}_{02}$ to terminal set ${\mathbf{X}}_{T2}$ with traversability
map given by $b_{2}({\mathbf{x}})$. The control plots for the trajectories
given in Fig. 1 are displayed in Fig. 3. The control limits are set to be $\pm
3$ and are seen to be met by the control.

The optimal navigation problem given in (25a) - (25d) can also be utilized to
avoid any particular predetermined region such as enemy camp, lake or pits. We
show the optimal navigation plots without and with such a hard obstacle set
${\mathbf{X}}_{u}$ in Fig. 4(a) and Fig. 4(b) respectively. The corresponding
density plots ($\rho({\mathbf{x}})$) for the trajectories with and without
hard obstacles are shown in Fig. 5(a) and Fig. 5(b).

Figure 1: Optimal navigation with initial points centered around $(6.4,8.5)$
in initial set ${\mathbf{X}}_{01}$ to terminal set ${\mathbf{X}}_{T1}$ with
traversability map given by $b_{1}({\mathbf{x}})$. Figure 2: Optimal
navigation with initial points centered around $(-0.5,-1.5)$ in initial set
${\mathbf{X}}_{02}$ to terminal set ${\mathbf{X}}_{T2}$ with traversability
map given by $b_{2}({\mathbf{x}})$. Figure 3: Optimal control plots for
$u_{1}$ corresponding to different initial conditions with configuration of
initial and final sets as in Fig. 1.

(a) Optimal navigation with initial points centered around $(5.5,7.5)$ in
initial set ${\mathbf{X}}_{01}$ to terminal set ${\mathbf{X}}_{T1}$ with
traversability map given by $b_{1}({\mathbf{x}})$.

(b) Optimal navigation with initial points centered around $(5.5,7.5)$ in
initial set ${\mathbf{X}}_{01}$ to terminal set ${\mathbf{X}}_{T1}$ with
traversability map given by $b_{1}({\mathbf{x}})$.The traversability map
includes hard obstacle centered around $(3,4)$.

Figure 4: Navigation with and without hard obstacles.

(a) Density plots for the trajectories shown in Fig. 4(a).

(b) Density plots for the trajectories shown in Fig. 4(b).

Figure 5: Density plots for terrain navigation with (5(b)) and without (5(a))
hard obstacles.

##  VII Conclusion

The problem of navigation on off-road terrain is considered. We use
traversability measure to describe the relative degree of difficulty of
navigation. A convex formulation for the optimal navigation problem is
constructed using the traversability information of the terrain. The convex
formulation leads to an infinite-dimensional convex optimization problem for
navigation. Furthermore, we utilize the data-driven approximation of the
linear P-F operator for the finite-dimensional approximation of the
optimization problem. Finally, simulation results are showcased to show the
validity of the proposed method. Future research efforts will incorporate the
uncertainty arising from vehicle dynamics and vehicle-terrain interaction in
the framework.

## References

  * [1] M. W. Hirsch, S. Smale, and R. L. Devaney, _Differential equations, dynamical systems, and an introduction to chaos_. Academic press, 2012.
  * [2] D. E. Koditschek and E. Rimon, “Robot navigation functions on manifolds with boundary,” _Advances in applied mathematics_ , vol. 11, no. 4, pp. 412–442, 1990.
  * [3] S. Mentasti and M. Matteucci, “Multi-layer occupancy grid mapping for autonomous vehicles navigation,” in _2019 AEIT International Conference of Electrical and Electronic Technologies for Automotive (AEIT AUTOMOTIVE)_. IEEE, 2019, pp. 1–6.
  * [4] T. Overbye and S. Saripalli, “Fast local planning and mapping in unknown off-road terrain,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 5912–5918.
  * [5] B. Gao, A. Xu, Y. Pan, X. Zhao, W. Yao, and H. Zhao, “Off-road drivable area extraction using 3d lidar data,” in _2019 IEEE Intelligent Vehicles Symposium (IV)_. IEEE, 2019, pp. 1505–1511.
  * [6] Y. Yang, D. Tang, D. Wang, W. Song, J. Wang, and M. Fu, “Multi-camera visual slam for off-road navigation,” _Robotics and Autonomous Systems_ , vol. 128, p. 103505, 2020.
  * [7] M. Bajracharya, A. Howard, L. H. Matthies, B. Tang, and M. Turmon, “Autonomous off-road navigation with end-to-end learning for the lagr program,” _Journal of Field Robotics_ , vol. 26, no. 1, pp. 3–25, 2009.
  * [8] A. Hussein, P. Marín-Plaza, D. Martín, A. de la Escalera, and J. M. Armingol, “Autonomous off-road navigation using stereo-vision and laser-rangefinder fusion for outdoor obstacles detection,” in _2016 IEEE Intelligent Vehicles Symposium (IV)_. IEEE, 2016, pp. 104–109.
  * [9] T. Overbye and S. Saripalli, “G-vom: A gpu accelerated voxel off-road mapping system,” _arXiv preprint arXiv:2109.13176_ , 2021.
  * [10] D. Hoeller, L. Wellhausen, F. Farshidian, and M. Hutter, “Learning a state representation and navigation in cluttered and dynamic environments,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 5081–5088, 2021\.
  * [11] B. Huang and U. Vaidya, “A convex approach to data-driven optimal control via perron-frobenius and koopman operators,” _arXiv preprint arXiv:2010.01742; (accepted for publication in IEEE Transactions on Automatic Control)_ , 2020.
  * [12] H. Yu, J. Moyalan, U. Vaidya, and Y. Chen, “Data-driven optimal control of nonlinear dynamics under safety constraints,” _IEEE Control Systems Letters_ , 2022.
  * [13] U. Vaidya, “Optimal motion planning using navigation measure,” _International Journal of Control_ , vol. 91, no. 5, pp. 989–998, 2018.
  * [14] J. Moyalan, H. Choi, Y. Chen, and U. Vaidya, “Sum of squares based convex approach for optimal control synthesis,” in _2021 29th Mediterranean Conference on Control and Automation (MED)_. IEEE, 2021, pp. 1270–1275.
  * [15] J. Moyalan, Y. Chen, and U. Vaidya, “Navigation with probabilistic safety constraints: Convex formulation,” in _accepted to be presented at American Control Conference 2022_. IEEE, 2022.
  * [16] A. Lasota and M. C. Mackey, _Chaos, fractals, and noise: stochastic aspects of dynamics_. Springer Science & Business Media, 1998, vol. 97.
  * [17] H. Huang, A. V. Savkin, and C. Huang, “Decentralized autonomous navigation of a uav network for road traffic monitoring,” _IEEE Transactions on Aerospace and Electronic Systems_ , vol. 57, no. 4, pp. 2558–2564, 2021.
  * [18] C. Armbrust, T. Braun, T. Föhst, M. Proetzsch, A. Renner, B.-H. Schäfer, and K. Berns, “Ravon: The robust autonomous vehicle for off-road navigation,” in _Using Robots in Hazardous Environments_. Elsevier, 2011, pp. 353–396.
  * [19] N. Bagheri, “Development of a high-resolution aerial remote-sensing system for precision agriculture,” _International journal of remote sensing_ , vol. 38, no. 8-10, pp. 2053–2065, 2017.
  * [20] X. Xiao, J. Biswas, and P. Stone, “Learning inverse kinodynamics for accurate high-speed off-road navigation on unstructured terrain,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 6054–6060, 2021.
  * [21] J. M. Gregory, G. Warnell, J. Fink, and S. K. Gupta, “Improving trajectory tracking accuracy for faster and safer autonomous navigation of ground vehicles in off-road settings,” in _2021 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR)_. IEEE, 2021, pp. 204–209.
  * [22] I. Barbalat, “Systemes d’équations différentielles d’oscillations non linéaires,” _Rev. Math. Pures Appl_ , vol. 4, no. 2, pp. 267–270, 1959.
  * [23] A. Balluchi, A. Bicchi, A. Balestrino, and G. Casalino, “Path tracking control for dubin’s cars,” in _Proceedings of IEEE International Conference on Robotics and Automation_ , vol. 4. IEEE, 1996, pp. 3123–3128.
  * [24] B. Huang and U. Vaidya, “Data-driven approximation of transfer operators: Naturally structured dynamic mode decomposition,” in _2018 Annual American Control Conference (ACC)_. IEEE, 2018, pp. 5659–5664.
  * [25] M. O. Williams, I. G. Kevrekidis, and C. W. Rowley, “A data–driven approximation of the koopman operator: Extending dynamic mode decomposition,” _Journal of Nonlinear Science_ , vol. 25, no. 6, pp. 1307–1346, 2015.
  * [26] M. Korda and I. Mezić, “On convergence of extended dynamic mode decomposition to the koopman operator,” _Journal of Nonlinear Science_ , vol. 28, no. 2, pp. 687–710, 2018.
  * [27] S. Klus, I. Schuster, and K. Muandet, “Eigendecompositions of transfer operators in reproducing kernel hilbert spaces,” _Journal of Nonlinear Science_ , vol. 30, no. 1, pp. 283–315, 2020.


∎

11institutetext: N. Chopra 22institutetext: Department of Physics, University
of Warwick, Coventry, United Kingdom 22email<EMAIL_ADDRESS>33institutetext: J. Lloyd-Hughes 44institutetext: Department of Physics,
University of Warwick, Coventry, United Kingdom 44email: j.lloyd-
<EMAIL_ADDRESS>
# Optimum optical designs for diffraction-limited terahertz spectroscopy and
imaging systems using off-axis parabolic mirrors

Nishtha Chopra  James Lloyd-Hughes

(Received: date / Accepted: date)

###### Abstract

Off-axis parabolic mirrors (OAPMs) are widely used in the THz and mm-wave
communities for spectroscopy and imaging applications, as a result of their
broadband, low-loss operation and high numerical apertures. However, the
aspherical shape of an OAPM creates significant geometric aberrations that
make achieving diffraction-limited performance a challenge, and which lowers
the peak electric field strength in the focal plane. Here we quantify the
impact of geometric aberrations on the performance of the most widely-used
spectrometer designs, by investigating whether diffraction-limited performance
can be achieved in both the sample and the detector plane. We identify simple
rules, based on marginal ray propagation, that allow spectrometers to be
designed that are more robust to misalignment errors, and which have minimal
aberrations for THz beams. For a given source this allows the design of
optical paths that give the smallest THz beam focal spot, with the highest THz
electric field strength possible. This is desirable for improved THz imaging,
for better signal-to-noise ratios in linear THz spectroscopy and optical-pump
THz-probe spectroscopy, and to achieve higher electric field strengths in non-
linear THz spectroscopy.

###### Keywords:

Terahertz off-axis parabolic mirror ray tracing

##  1 Introduction

Within the research community, different optical setups containing off-axis
parabolic mirrors (OAPMs) are widely utilized to characterise broadband THz
radiation sources Mosley2017 ; Rouzegar2023 ; chopra , as well as to perform
linear THz spectroscopy in the transmission or reflection geometry
lee2009principles ; Sun2018 , non-linear THz spectroscopyReimann2021 ,
optical-pump THz-probe spectroscopy (OPTPS) Ulbricht2011  and near-field THz
microscopy Lloyd-Hughes2021 . The optimum performance for a THz spectrometer
will be achieved if an aberration-free image of the THz source can be formed
at each focal plane. For linear THz spectroscopy, a minimal spot size is often
desired: to investigate samples that are small in transverse extent Jones2014
; to couple more effectively to sub-wavelength structures and waveguidesLloyd-
Hughes2009 ; or to image spatially inhomogeneous materials, such as large-area
graphene Tomaino2011 . Further, the signal detected in the detector plane
(_e.g._ via electro-optic sampling or photoconductive detection) in THz time-
domain spectroscopy is proportional to the electric field of the THz beam
(rather than the area- and time-integrated power, as in a bolometer or
pyroelectric), and is hence larger when the THz beam has a smaller area. In
OPTPS, the THz probe beam must have a smaller spatial extent than the optical
pump beam, in order to probe a uniform carrier density: it is therefore also
desirable to have a smaller THz beam so this condition can be readily achieved
for lower power optical pump lasers Butler-Caddle2023 . Finally, intense THz
pulses with high electric field strengths (typically $>100$ kV/cm) can be used
to study the non-linear dynamical motion of vibrational modes or free charges
Reimann2021 . For non-linear THz spectroscopy, it is thus important to
efficiently focus one or more THz beams down to as close to the diffraction
limit as possible, to obtain the highest electric field strength.

The majority of sources of broadband THz radiation (such as photoconductive
antennae, optical rectification in non-linear crystals and spintronic
emitters) produce THz beams that propagate in free space in the fundamental
(TEM00) transverse Gaussian mode. The Gaussian nature of terahertz (THz) beams
has been corroborated through a combination of theoretical and experimental
techniques, wherein, scalar and vectorial diffraction theory has been utilized
to analyze the far-field behaviour of the THz pulses Gurtler:00 . Similarly,
experimental validation was achieved by imaging the spatial and temporal
distribution of pulses, in a standard THz-TDS setup Jiang:99 . In free space,
the divergence angle of this Gaussian mode is $\theta=\lambda/\pi w_{0}$ in
the paraxial approximation, where $w_{0}$ is the radius of the initial beam
waist. For example, $\theta=18^{\circ}$ for light at 1 THz ($\lambda=300$
$\mu$m) for $w_{0}=300$ $\mu$m, and the divergence angle increases for longer
wavelengths or smaller initial beam sizes. Hence, THz radiation tends to
diverge rapidly, and large numerical aperture optics are required, in
particular, to collect low-frequency components with longer wavelengths
peiponen2012terahertz . While polymer lenses are widely adopted in many THz
systems due to their light weight and ease of fabrication lee2009principles ,
their finite Fresnel losses and absorption reduces optical throughput, and
they can introduce optical aberrations.

To overcome these challenges, high-reflectivity metal-coated off-axis
parabolic mirrors (OAPMs) are widely used peiponen2012terahertz . An OAPM is a
segment of a parent parabolic surface and can be defined by the off-axis
angle, reflected effective focal length $f$ and diameter $D$, as illustrated
in Fig. 1. These mirrors offer broadband performance while eliminating Fresnel
losses, and can collimate a beam diverging from an on-axis point source
without aberrations, or focus a collimated beam to a diffraction-limited spot.
In the geometric optics (ray optics) picture, an OAPM can perfectly collimate
rays of light that originate from a point source only if the point source is
on the optical axis and in the focal plane, at a distance $f$ from the mirror
(blue rays in Fig. 1). However, real THz sources have a finite spatial extent,
set by the size of the incident laser beam or the emitter’s active area in
laser-based THz systems, and the propagation of light from off-axis points in
the object plane must also be considered (green rays in Fig. 1).

Different optical analysis techniques have been deployed to model OAPM
systems, such as deriving analytical expressions for Hermite-Gaussian beam
propagation after reflection from an OAPM in the paraxial approximation
Murphy1987  or by using optical ray tracing methods Arguijo2003 ; Bruckner2010
. In the former case, after a fundamental Gaussian beam was incident onto an
OAPM, the reflected electric field was found to have a skewed and broadened
profile corresponding to the presence of additional higher order (TEM30 and
TEM12) Hermite-Gaussian modes Murphy1987 . Importantly, by adding a second
OAPM placed at the correct distance and with the right orientation, it was
shown that the distortions introduced by the second mirror can cancel out the
distortions introduced by the first mirror, resulting again in a clean
fundamental Gaussian beam (desirable in order to achieve a diffraction-limited
spot). Alternatively, analytical ray tracing was used to show that if a
collimated beam of light is incident onto an OAPM at an angle away from its
optical axis, the focal spot is distorted Arguijo2003 .

Figure 1: Geometry of a right-angle OAPM.

While analytical approaches are useful for specific cases where the underlying
assumptions are valid, they become cumbersome for realistic optical systems
containing many OAPMs. An alternative approach is to use ray tracing software,
in which point sources can be readily placed at different positions to assess
the optical performance for objects with finite extent, or for point sources
that are placed off-axis. Further, an arbitrary number of optical components
can be included. Brückner _et al._ used the ZEMAX ray tracing software to show
that for an off-axis point source, the beam after a single right-angled OAPM
has an astigmatic wavefront Bruckner2010 . Further, and similar to the
conclusion derived in Murphy’s work using analytical theory Murphy1987 ,
Brückner _et al._ demonstrated that a judiciously-oriented second OAPM can
cancel out the astigmatic wavefront, leading to diffraction-limited
performance for off-axis field points. It was shown that the second OAPM,
which was placed in a 4f geometry, has to be oriented to send the reflected
beam anti-parallel to the original beam direction (i.e. along $-z$ if the THz
source radiates along $z$) in order to substantially cancel out aberrations
for off-axis field points. This was in contrast to the alternative orientation
for the second OAPM, reflecting the THz beam along $+z$, in which aberrations
were evident.

Based on these considerations, Laurita _et al._ subsequently performed an
experimental comparison of the two geometries and showed that the beam waist
at the sample focus of a typical THz-TDS system was smaller, at around 7 mm
compared to 11 mm, for the aberration-corrected arrangement than the
alternative orientation laurita2016modified . However, diffraction-limited
performance was not achieved and the performance of the different OAPM
arrangements in the detector plane was not considered. Recently, the present
authors experimentally investigated a linear array of photoconductive THz
emitters, where each pixel acted as a THz source at a different distance from
the optical axis chopra , and reported close to the diffraction-limited
performance obtained using an arrangement of OAPMs that corrected for
aberrations.

Given the above challenges for optical systems containing OAPMs, it is timely
and pertinent to consider how best to minimise geometric aberrations, such
that diffraction-limited performance can be achieved at both the sample
position and the detector position in THz spectrometers. In this Article, we
provide a comprehensive analysis of how the design of a spectrometer impacts
its performance, with particular emphasis on the spatial and temporal spread
in the sample and detector focal planes. In Section 2 we describe the
modelling approach taken: we used the ZEMAX optical design software to ray
trace the beam path for the two optical systems most commonly used in THz
spectroscopy, as well as an uncommon but optimum design, starting from a
variety of on- and off-axis points. Further, we introduce a notation that
captures the relative orientation of different OAPMs based on marginal ray
propagation. We then, in Section 3, examine and evaluate the performance of
the different spectrometer designs by using spatial plots of ray propagation,
and optical path differences in the time-domain. In Section 4 we conclude our
analysis by ranking the three competing optical designs in terms of their
robustness to misalignment, and their ability to form accurate images of the
THz source in the sample plane and the detector plane. Finally, we use the
insights gained from ray tracing and marginal ray propagation to predict the
optimum arrangement of OAPMs for a reflection geometry setup.

##  2 Modelling approach and spectrometer designs

In this Section we define the ray-tracing methodology used and describe the
key nomenclature used in the optical design field. Illustrated in Fig. 1 is a
right-angled mirror viewed in the $y-z$ plane, also termed the tangential
plane. The $x-z$ plane is referred to as the sagittal plane. The ray that
propagates from the focus in the emitter plane along $z$, along the optical
axis of the OAPM, is referred to as the _chief ray_ : it changes direction by
90∘ after hitting the geometric centre of the OAPM, to travel along $y$.
_Marginal rays_ are incident on the edges of the OAPM, and span the maximum
aperture as seen from the geometric centre. The asymmetric shape of the OAPM
is particularly evident when considering the two marginal rays that hit the
closest side (at point $a$) or the furthest side (point $b$) of the OAPM, as
the two marginal rays travel at different angles to the chief ray. Here, all
OAPMs were assumed to be right-angled, with $f=76.2$ mm and $D=50.8$ mm,
representative of the mirrors most often used. For this choice, the numerical
aperture was 0.31, and the angle of the marginal rays to the chief ray was
$\alpha\simeq\tan^{-1}(D/2f)=18.4^{\circ}$. This is similar to the divergence
calculated above for a 1 THz Gaussian beam, suggesting that the majority of
the THz radiation produced by the emitter would be collected by this OAPM, and
that the OAPM would be filled effectively such that smaller diffraction-
limited beam focii can be achieved.

Typical THz spectroscopy and imaging systems require at least two pairs of two
OAPMs: the first pair collects the THz radiation from the source, and focuses
it onto a sample, while the second pair collects the transmitted or reflected
THz beam and focuses it onto a detector. There are a substantial number of
degrees of freedom possible for a complete spectrometer design, as each
individual OAPM has six positional degrees of freedom (three translational and
three rotational). Here we examined the most common spectrometer designs,
which consist of four right-angled OAPMs, with their optical axes all within
the same plane (here denoted $y-z$). For each OAPM added to an optical setup
there are two options for its orientation that keep the optical axis in the
same plane: these change the beam direction by $\pm 90^{\circ}$ relative to
its previous direction in the $y-z$ plane.

To describe three different geometries reported in the literature we define
the nomenclature U-shape, step-shape and S-shape to refer to the spectrometers
illustrated in Figs. 2(a)-(c), based on the pattern formed when considering
the chief ray’s beam path (yellow lines and arrows). The U-shape and step-
shape design are the most widely used in THz time-domain spectrometers, while
the S-shape was suggested to minimise aberrations in the sample plane
laurita2016modified . Laurita _et al._ compared the performance of the U-shape
and S-shape (referred to as the “conventional” and “modified” designs in their
work), but did not consider the step-shape or the performance in the detector
plane. In the U-shape and step-shape designs [Figs. 2(a)-(b)], the first two
OAPMs are oriented identically, but the designs differ in how the second pair
of OAPMs is oriented. For the U-shape, the second pair of OAPMs is oriented to
return the THz beam to the same $y$ co-ordinate, leading to a relatively
compact design, while the step- and S-shapes have a larger size in $y$.

We stress here that the U-shape does not have the correct layout to collect
all the _on-axis_ rays after the second OAPM: across the sample plane, the
marginal rays highlighted in red are _not_ collected by the third OAPM [Figs.
2(a)], and hence the throughput of the system is not perfect. This problem is
not seen for the step-shape or S-shape, where the third OAPMs are oriented
correctly, and all the marginal rays after the sample plane are collected.

Figure 2: Ray tracing models of three different THz-TDS geometries for an on-
axis point source at centred at $(0,0)$ in the emitter plane (EP), propagating
to the sample plane (SP) and detector plane (DP). (a) U-geometry, or
$(a,b)(a,b)$ using the notation defined in the text. The red highlighted
marginal rays are not captured as they propagate. (b) Step-geometry, or
$(a,b)(b,a)$ arrangement. The second OAPM pair is oriented the same as the
first. (c) S-geometry, or $(a,a)(a,a)$ setup. (d) For the U- and step-shapes,
propagation from an off-axis $(0,2)$ source, shown as green rays, leads to a
tangential plane focus behind the SP focus for on-axis propagation, thus
creating aberrations. (e) For the sample plane of the S-geometry, the off-axis
rays rays from $(0,2)$ converge in the SP with minimal aberration.

Based on a consideration of the marginal rays and the symmetry properties of
an OAPM system, we define here a nomenclature that we find useful to describe
the optical arrangement of pairs of OAPMs, inspired by the Glazer and
Aleksandrov notation schemes widely used to describe the tilt patterns of
perovskite octahedra. As we show in the following section, the orientation of
each OAPM within a pair and the relative orientation of each pair of OAPMs are
both critical to achieve minimal aberrations. Therefore, we introduce the
notation $(x_{i},x_{j})$ to denote the orientation of a pair of OAPMs, where
$x=a,b$ to denote whether or not the marginal ray reflects from the “near”
side or “far” side points $a$ and $b$ in Fig. 1, and the subscript denotes the
focal lengths $f_{i}$ and $f_{j}$. If the focal lengths of all mirrors are
identical, as in this work, we drop the subscript notation. With this scheme,
the U-shape design can be written $(a,b)(a,b)$ as pictured by the cyan ray in
Fig. 1(a), as the marginal ray that reflects from point $a$ on the first OAPM
then reflects from $b$, $a$ and $b$ points on the subsequent mirrors. The
step-shape can be written $(a,b)(b,a)$ and the S-shape is $(a,a)(a,a)$.
Retaining the brackets is useful as a guide to show where the focii of the
optical system are, namely before and after each bracket.

To assess the performance of each setup, we introduced an offset in the
position of the point source in the emitter (object) plane, either in the
$y$-direction, as illustrated in Fig. 1, in the $x$ direction, or in both. As
a consequence of the point source being off-axis by a distance $\delta=2$ mm,
rays propagate at a slight angle 1.5∘ after the first OAPM, in comparison to
the on-axis rays. This tilt can be observed in Fig. 2(d), where the off-axis
rays are represented in green. The second OAPM, which forms an $(a,b)$
orientation pair, focused rays to a point at a distance $-\delta$ in $y$ from
the optical axis, where the negative sign signifies that the image formed
after the two OAPMs was inverted, and at a distance along the $z$ axis after
from the SP. In the sagittal plane (not shown here) the focus forms before the
SP. Notably, for the alternative orientation of the second OAPM, $(a,a)$,
pictured in Fig. 2(e), the off-axis rays are refocussed at the SP. In order to
better investigate the impact of OAPM orientation we now consider the spatial
and temporal performance of the different designs in more detail.

##  3 Results

In a ray-traced model of an optical system, whether or not an optical system
can achieve diffraction-limited performance can be assessed by examining “spot
diagrams,” which show the spatial distribution of rays as they intersect the
focal plane, and comparing it to the diffraction-limited beam size, for
instance, given by an Airy disk. Alternatively, because ray tracing keeps
track of the phase of the electric field, the optical path difference (OPD) of
different rays can be determined relative to the chief ray. The system can be
assumed to be diffraction limited if the phase change for all rays is
sufficiently small: an OPD of less than a quarter wavelength is a common
convention used in the optical modelling community to define diffraction-
limited performance. To comprehensively assess image quality, we conducted a
comparative analysis of the spatial distribution and the temporal evolution of
the THz field across all geometries, at a frequency of 1 THz (wavelength
$\lambda=300\mu$m). Note that the only impact of the wavelength is that it
changes the diameter of the diffraction-limited spot size.

###  3.1 Spatial performance

Spot diagrams provide essential visual indicators to assess the symmetry of
aberrations and image quality, and are reported in Fig. 3 for each geometry
(three rows) and at both the sample plane (left-hand column) and in the
detector plane (right-hand column). In each case, the sequence of spot
diagrams shows how the beam propagates through the focus in steps of 1 mm
along the beam propagation direction. Different colours represent the spot
diagrams obtained for on-axis and off-axis point sources. The spot diagrams
are made up of a large number of spots, not all of which are separable on the
scale shown here (10 mm by 10 mm for each spot diagram), where 1 mm
corresponds to one of the smaller grey squares. It is evident for all
geometries that propagation from an on-axis point source (blue points) is
well-behaved, in that the rays traverse the system and reach a point-like
focus both in the SP and the DP.

Figure 3: Spatial profile of the THz beam near the sample plane (left column)
and detector plane (right column), for the three different designs (rows).
Within each black rectangle, spot diagrams are shown for a particular plane
and design, as a function of the offset of the source in the emitter plane and
the “defocus” - the difference along $z$ from the ideal focus. The colours and
far right-hand labels indicates the $(x,y)$ offsets of the point source in the
emitter plane (in mm). Every plot has an Airy-disk (black line) that provides
a visual indicator of the diffraction-limited beam size at 1 THz
($\lambda=300\mu$m) for an entrance pupil diameter of 50.8 mm. The U-geometry
has significant aberrations for a $2$ mm offset, both at the SP and at the DP:
rays fall outside the Airy disk limit, and the image is distorted. The DP
performance of the step-geometry is poor at the SP but good at the DP, while
the S-shape rays fall within the Airy disk for both SP and DP, and it exhibits
the least aberrations.

Considering the spot diagrams for the SP for the U-shape and the step-shape
(top-left and middle-left black rectangles), which are identical owing to
sharing the same initial $(a,b)$ geometry, it is clear that a small offset of
2 mm in $x$ or $y$ is sufficient to substantially distort the shape of the
beam through the focus. The offset in $y$ (green) creates a beam that is
narrowest in $x$ at a defocus of -2 mm along the beam direction, while the
narrowest beam width in $y$ lies around a defocus of 2 mm (corresponding to
the case shown in Fig. 2(d)). In either case the minimum beam waist is around
2 times the diffraction-limited size at 1 THz, as evident from a comparison
with the black circles (Airy disks), which have radius
$\phi_{r}=1.22\lambda/f_{\\#}$ for f-number $f_{\\#}$. The system does not
have diffraction-limited performance at 1 THz or higher frequencies, as the
Airy disk reduces in diameter at higher frequencies.

In contrast, for the S-shaped design, with $(a,a)$ OAPMs, the performance at
the sample position (bottom-left) is excellent for all the off-axis points
pictured. The reason for the better performance can be intuitively understood
as follows: at the second OAPM the marginal rays hit a matched surface, such
that the distortion in the wavefront introduced by the first OAPM can be
“inverted” by the second OAPM, as the beam propagates to the focus.
Considering the $(a,a)$ configuration further, we found it continued to
achieve diffraction-limited performance for offsets as large as $\pm 4$ mm,
although for larger offsets (_e.g._ 8 mm) the beam tilt in the tangential
plane in the collimated section became too substantial and a large fraction of
marginal and central rays were lost.

We continue to discuss the spot diagrams in Fig. 3 by considering the right-
hand column, which illustrates the spot diagrams close to the detector plane
for the same point source offsets and defocus positions as discussed above.
From these diagrams, it is clear that the U-shape and step-shape designs have
substantially different imaging performances at the detector plane. For the
U-shape, the aberrations evident at the SP are further compounded by the
second OAPM pair, such that the off-axis spots are even larger in the DP. In
contrast, for the step shape geometry, the DP performance is diffraction
limited and comparable to that of the S-shape.

###  3.2 Temporal performance

Spot diagrams offer a purely geometric perspective on aberrations, providing
insights into their spatial characteristics. However, for a more comprehensive
understanding, it proves advantageous to investigate the optical path
difference (OPD) of the system relative to the ideal wavefront surface, taken
to be a sphere with radius $f$, centred at the chief ray’s position in the
image plane. The OPD is then calculated for the different rays and is shown
graphically by plotting it as a function of the position normalised by the
radius of the last optic in the plane of the exit pupil, where $P_{x}$ and
$P_{y}$ correspond to the exit pupil’s coordinates in the sagittal and
tangential planes, respectively. In Figure 4 we present the OPD for the sample
plane versus $P_{x}$ (top row) and $P_{y}$ (bottom row), for various point
source positions in the emitter plane (left to right), and for each design.
While the OPD is often shown as the path difference divided by the wavelength,
we opted here to show the OPD as a time delay to allow a better understanding
of the scale of the OPD changes for the THz time-domain spectroscopy
community.

For the on-axis case, $(0,0)$, the horizontal lines in the top-left and
bottom-left panels in Fig. 4 signify the absence of an optical path difference
(OPD) for all designs, i.e. zero phase difference, representing a perfect
spherical wavefront converging to a point image. The off-axis points
(representing translational misalignment or finite beam size) result in the
incident wavefront converging at the focal point with a slight tilt caused by
either the delayed or advanced arrival of these rays. For the $(0,2)$ field
point a parabola-like OPD can be seen in $P_{x}$ and $P_{y}$ for the step and
U geometries, with large OPD values $>500$ fs for the marginal rays (rays at
large positive or negative values of $P_{x}$ or $P_{y}$). In contrast, for
S-geometry, the OPD change is small.

For THz time-domain spectroscopy applications, it is important to assess how
the different path lengths impact upon the THz pulse duration at the sample
position. One could average over the OPD curve by using an assumed profile for
the THz beam at the exit pupil (_e.g._ a Gaussian) to weight the amplitude for
each ray. Rather than perform that exercise, which would be wavelength and
beam specific, we instead highlight the typical pulse duration of the fs laser
pulses ($\sim 100$ fs) used in THz time-domain spectroscopy by the horizontal
dashed lines. An OPD larger than this limit will introduce a substantial phase
shift, and hence will broaden the duration of a THz pulse, simultaneously
lowering the peak amplitude of the THz electric field. It is clear from Fig. 4
that for off-axis points (e.g. from poor emitter alignment) the temporal
duration of the THz pulse at the sample plane is likely to be longer in
duration for the step- and U-geometries, as a result of geometric aberrations.
Further, it can be seen that precise alignment in the $y$ direction is more
critical (the OPDs are smaller for offsets in $x$).

Figure 4: Wavefront distortion illustrated by the optical path difference for
the first two OAPMs. The plot presents the tangential ($P_{y}$) and sagittal
plane ($P_{x}$) optical path differences at the exit pupil. The dashed red
line denotes the typical fs laser pulse duration (100 fs) used in THz-TDS
setups. The OPD is not defined for exit pupil coordinates where there are no
rays on the final OAPM (because of beam tilt), for example along $P_{y}$ in
the S-geometry for $(0,2)$.

The OPD for the complete 4 OAPM optical setups is illustrated in Fig. 5, which
provides a clear depiction of why the aberrations in the DP differ for the
three geometries. The OPD for off-axis point sources is largest for the
U-geometry, reaching as large as 1 ps (green curves), suggesting that the
U-shape will temporally broaden the THz pulse at the detection position as a
result of the substantial geometric aberrations. In comparison, the step-shape
(orange curves) and S-geometry (blue curves) have a phase difference near
zero, and within the $\pm 100$ fs limit that we suggest is important for
typical THz time-domain spectrometer systems.

While the excellent performance of the S-geometry can be assumed to result
from the cancellation of distortions within each pair in the $(a,a)(a,a)$
orientation, the similar performance of the step-shape at the detector
position, and the poor performance of the U-shape need further explanation.
The relative performance can be explained by considering the OPD introduced by
each OAPM pair, as well as keeping track of the relative orientation of each
pair using the marginal ray notation introduced above: $(a,b)(b,a)$ for the
step-shape and $(a,b)(a,b)$ for the U-shape. For the first OAPM pair, the
$(a,b)$ orientation creates a distorted wavefront for an off-axis field point,
for example, the roughly parabolic OPDs in $P_{x}$ and $P_{y}$ seen in Fig. 4
for a $(0,2)$ point source in the emitter plane. In the step-shape, the second
OAPM pair has $(b,a)$ orientation, and is in the same orientation in the lab
coordinate system as the first pair. However, the OPD that the $(b,a)$ pair
produces is not the same as for the first $(a,b)$ pair, because the image is
inverted from the EP to the SP: e.g. the $(0,2)$ point in the emitter plane
maps to $(0,-2)$ in the sample plane. Critically, the OPD flips in sign for
inverted field points in the $x-y$ plane Bruckner2010 . Thus the OPD (or
wavefront distortion) created by the second, $(b,a)$ pair in the step-geometry
acts to cancel out the OPD of the first pair. In contrast, if the second pair
has $(a,b)$ orientation, as in the U-shape, the OPD of the second pair has the
same sign, and the wavefront distortions add rather than subtract.

Figure 5: Wavefront distortion illustrated by the optical path difference for
the entire optical path to the detection plane. The S- and step-geometry (blue
and orange respectively) demonstrate a superior performance for off-axis
points, showing minimal path difference and the least temporal broadening. On
the other hand, for the U-geometry, we observe that translational misalignment
rapidly cause wavefront aberrations leading to large OPDs.

##  4 Discussion

From the results presented above we can conclude that the S-shape geometry
should be adopted in linear, non-linear and OPTP spectroscopy applications, as
it is the most robust to the misalignment of the THz source in the $x-y$
plane, and can best cancel out aberrations introduced by the OAPMs in order to
achieve a diffraction-limited performance in both the SP and DP. For linear
THz spectroscopy, how critical it is to achieve diffraction-limited
performance at the intermediate focal plane (the sample position) depends on
the application: if raster scanned images are required, or if samples with
small $x-y$ extent are studied, then it is clearly advantageous to also work
with the S-shape geometry. However, if only spectroscopic measurements are
desired, and the highest peak electric field/smallest beam area at the sample
position is not critical, then the step-shape can be alternatively used as it
has comparable performance to the S-shape in the DP.

While the magnitude of the geometric aberrations in the SP and DP are
properties of the spectrometer design and the EP offset, whether or not
diffraction-limited performance can be achieved depends on the wavelength and
it is therefore useful to consider the frequency-dependent performance of each
design. For example, diffraction-limited performance is not achieved at 1 THz
in the SP for the U- and step-geometries (Fig. 3) for a 2 mm offset, because
geometric aberrations cause rays to fall over a region about twice the
diameter of the Airy disk. However at 500 GHz, where the Airy disk doubles in
radius relative to that at 1 THz, the majority of rays would fall within the
Airy disk and diffraction-limited performance would be achieved at the SP. For
higher THz frequencies, the Airy disk reduces in diameter and the relative
contribution of aberrations becomes more significant.

Based on a consideration of the spot diagrams and OPDs presented above, we can
now suggest rules to aid the design of OAPM systems for THz spectroscopy and
imaging:

  1. 1.

geometric aberrations may be expected to impact the optical performance of
typical OAPM systems at 1 THz and higher frequencies;

  2. 2.

an OAPM pair should have $(a,a)$ orientation to minimise aberrations at the
focus;

  3. 3.

if an $(a,b)$ orientation is imposed by some other experimental constraint,
then it will have aberrations in its image plane;

  4. 4.

these aberrations can be inverted by a $(b,a)$ pair.

These rules allow the optimum OAPM orientation to be deduced for more complex
spectrometer designs, without needing to ray-trace the geometry. For example,
if an additional optic such as a mirror is required to change the THz beam
direction, or to couple an optical beam along the optical axes of the OAPMs
(using a THz mirror that passes visible light and reflects THz radiation),
then it is desirable to know the optimum orientation for the OAPM(s) after the
THz mirror. We now consider the setup pictured in Fig. 6, where a mirror is
placed after the first OAPM, and the THz beam propagates along $z$. To form a
focus, two orientations of the second OAPM are possible (top and bottom left
diagrams). To identify the correct orientation of the final OAPM, such that
aberrations are minimised, marginal ray notation and the rules above are
useful. According to these rules the $(a,a)$ configuration (bottom left)
should be used as it will have less significant geometric aberrations than the
$(a,b)$ configuration (top left). Indeed the spot diagrams in the DP (right
hand panels) validate this assertion. Similar considerations can be applied to
correctly orient OAPMs in reflection geometry systems, where the THz beam
reflected from a sample is collected, and THz beamsplitters are used.

Finally, it is worth discussing the impact of geometric aberrations on
polarisation. There have been studies that reported asymmetric field and
polarization distributions for OAPMs with different f-numbers used in the U-
and step-shape Sung . Conversely, abrupt changes in THz polarization states
were experimentally observed by Takai _et al._ takai2016spatial , and were
ascribed to the inherent geometry of the OAPMs. In the present study, the
polarization performance of different OAPM geometries was not considered. We
note that in a recent experimental study using multi-pixel THz emitters
producing radial and azimuthal polarization, the polarization state can be
experimentally corrected Deveikis2022 .

Figure 6: Two OAPM geometry including a folding mirror, for a $(0,2)$ point
source. Top row: $(a,b)$ orientation (left) and through-focus spot diagrams
(right). Bottom row: as first row, but for the $(a,a)$ orientation of the
second OAPM.

##  5 Conclusion

The study presented here contributes to a better understanding of the
propagation and behaviour of THz pulses in multi-OAPM geometries. We
demonstrated that geometric aberrations can limit the optical performance both
in the sample and detector plane: they decrease the overall amplitude of the
THz field and increase the spot size. Depending on the specific orientation of
each OAPM in the optical system, the wavefront distortions, and aberrations at
each subsequent focus, can be reduced or enhanced. We introduced marginal ray
notation to capture the orientation of each OAPM in a system. The S-shape
cancels out the geometric aberrations by the correct orientation of the second
OAPM for each pair throughout the optical path, while the step-shape achieves
good performance for off-axis rays at the detection plane, by the second pair
of OAPMs cancelling out the distortion produced by the first pair. The use of
the U-shape design should be discouraged as it adds, rather than subtracts,
the distortion from each OAPM pair. Our modelling approach and design rules
can be readily applied to the design of more complex THz imaging and
spectroscopy setups based on OAPMs.

## Declarations

Ethical Approval. N/A

Competing interests. The authors declare that they have no competing
interests.

Authors’ contributions. N.C. performed the optical modelling, prepared the
figures, and drafted and edited the paper. J.L. drafted and edited the paper.

Funding. The authors would like to acknowledge funding from the EPSRC (UK)
(Grant No. EP/V047914/1).

Availability of data and materials. ZEMAX modelling files and data are
available from the authors on resasonable request.

## References

  * (1) C.D.W. Mosley, M. Failla, D. Prabhakaran, and J. Lloyd-Hughes.  Terahertz spectroscopy of anisotropic materials using beams with rotatable polarization.  Scientific Reports, 7(1):12337, 2017.
  * (2) R. Rouzegar, A.L. Chekhov, Y. Behovits, B.R. Serrano, M.A. Syskaki, C.H. Lambert, D. Engel, U. Martens, M. Münzenberg, M. Wolf, G. Jakob, M. Kläui, T.S. Seifert, and T. Kampfrath.  Broadband spintronic terahertz source with peak electric fields exceeding 1.5 MV/cm.  Phys. Rev. Appl., 19:034018, Mar 2023.
  * (3) N. Chopra, J. Deveikis, and J. Lloyd-Hughes.  Active THz beam shaping using a one-dimensional array of photoconductive emitters.  Applied Physics Letters, 122(6):061102, 02 2023.
  * (4) Y.-S. Lee.  Principles of terahertz science and technology, volume 170.  Springer Science & Business Media, 2009.
  * (5) Q. Sun, X. Liu, J. Cao, R.I. Stantchev, Y. Zhou, X. Chen, E.P.J. Parrott, J. Lloyd-Hughes, N. Zhao, and E. Pickwell-Macpherson.  Highly sensitive terahertz thin film total internal reflection spectroscopy reveals in situ photoinduced structural changes in methylammonium lead halide perovskites.  Journal of Physical Chemistry C, 2018.
  * (6) K. Reimann, M. Woerner, and T. Elsaesser.  Two-dimensional terahertz spectroscopy of condensed-phase molecular systems.  Journal of Chemical Physics, 154:120901, 3 2021.
  * (7) R. Ulbricht, E. Hendry, J. Shan, T.F. Heinz, and M. Bonn.  Carrier dynamics in semiconductors studied with time-resolved terahertz spectroscopy.  Reviews of Modern Physics, 83:543, 6 2011.
  * (8) J. Lloyd-Hughes, P. M. Oppeneer, T. Pereira Dos Santos, A. Schleife, S. Meng, M. A. Sentef, M. Ruggenthaler, A. Rubio, I. Radu, M. Murnane, X. Shi, H. Kapteyn, B. Stadtmüller, K. M. Dani, F. H. Da Jornada, E. Prinz, M. Aeschlimann, R. L. Milot, M. Burdanova, J. Boland, T. Cocker, and F. Hegmann.  The 2021 ultrafast spectroscopic probes of condensed matter roadmap.  Journal of Physics: Condensed Matter, 33:353001, 7 2021.
  * (9) S.P.P. Jones, S.M. Gaw, K.I. Doig, D. Prabhakaran, E.M. Hétroy Wheeler, A.T. Boothroyd, and J. Lloyd-Hughes.  High-temperature electromagnons in the magnetically induced multiferroic cupric oxide driven by intersublattice exchange.  Nature Communications, 5, 2014.
  * (10) J. Lloyd-Hughes, G. Scalari, A. Van Kolck, M. Fischer, M. Beck, and J. Faist.  Coupling terahertz radiation between sub-wavelength metal-metal waveguides and free space using monolithically integrated horn antennae.  Optics Express, 17(20):18387 – 18393, 2009.
  * (11) J. L. Tomaino, A. D. Jameson, J. W. Kevek, M. J. Paul, A. M. van der Zande, R. A. Barton, P. L. McEuen, E. D. Minot, and Y.-S. Lee.  Terahertz imaging and spectroscopy of large-area single-layer graphene.  Opt. Express, 19(1):141–146, Jan 2011.
  * (12) E. Butler-Caddle, N.E. Grant, S.L. Pain, J.D. Murphy, K.D.G.I. Jayawardena, and J. Lloyd-Hughes.  Terahertz photoconductance dynamics of semiconductors from sub-nanosecond to millisecond timescales.  Applied Physics Letters, 122:012101, 1 2023.
  * (13) A. Gürtler, C. Winnewisser, H. Helm, and P.U. Jepsen.  Terahertz pulse propagation in the near field and the far field.  J. Opt. Soc. Am. A, 17(1):74–83, Jan 2000.
  * (14) Z. Jiang and X.-C. Zhang.  2D measurement and spatio-temporal coupling of few-cycle THz pulses.  Opt. Express, 5(11):243–248, Nov 1999.
  * (15) K.-E. Peiponen, A. Zeitler, and M. Kuwata-Gonokami.  Terahertz spectroscopy and imaging, volume 171.  Springer, 2012.
  * (16) J. A. Murphy.  Distortion of a simple gaussian beam on reflection from off-axis ellipsoidal mirrors.  International Journal of Infrared and Millimeter Waves, 8:1165–1187, 9 1987.
  * (17) P. Arguijo and M.S. Scholl.  Exact ray-trace beam for an off-axis paraboloid surface.  Applied Optics, Vol. 42, Issue 16, pp. 3284-3289, 42:3284–3289, 6 2003.
  * (18) C. Brückner, G. Notni, and A. Tünnermann.  Optimal arrangement of $90^{\circ}$ off-axis parabolic mirrors in thz setups.  Optik, 121(1):113–119, 2010.
  * (19) N.J. Laurita, B. Cheng, R. Barkhouser, V.A. Neumann, and N.P. Armitage.  A modified 8f geometry with reduced optical aberrations for improved time domain terahertz spectroscopy.  Journal of Infrared, Millimeter, and Terahertz Waves, 37:894–902, 2016.
  * (20) S. Sung, J. Garritano, N. Bajwa, B. Nowroozi, W. Grundfest, and Z. Taylor.  Analysis and optimization of THz imaging optics with off-axis parabolic mirrors.  In 2014 39th International Conference on Infrared, Millimeter, and Terahertz waves (IRMMW-THz), pages 1–2, 2014.
  * (21) M. Takai, K. Shibata, M. Uemoto, and S. Watanabe.  Spatial polarization variation in terahertz electromagnetic wave focused by off-axis parabolic mirror.  Applied Physics Express, 9(5):052206, 2016.
  * (22) J. Deveikis and J. Lloyd-Hughes.  Multi-pixel photoconductive emitters for the controllable generation of azimuthal and radial terahertz beams.  Optics Express, 30:43293–43300, 11 2022.