\section{INTRODUCTION}
\noindent Time-optimal motion planning within cluttered environments poses multiple challenges. The underlying motion planning scheme needs to compute a set of input commands that drive the system from its current state to a goal location in minimum-time, without compromising system constraints and spatial bounds. Thus, its solution implies trading-off between time-optimality and spatial-awareness. 

To account for this, state-of-the-art approaches solve the motion planning problem in two separate stages \cite{tranzatto2022cerberus,foehn2022alphapilot,arrizabalaga2021caster}. First, the \emph{path planning} stage determines a collision-free geometric path according to high level -- task related -- commands \cite{gasparetto2015path}. Second, the predefined path is (exactly) tracked either by \emph{path tracking} or \emph{path following}. The former computes a dynamically feasible timing law for traversing along the predetermined geometric path -- \emph{when} to be \emph{where} --, while the latter introduces the timing law and the (bounded) distance to the path as control freedoms \cite{verscheure2009time,faulwasser2015nonlinear,lam2010model}.

When tackling time-optimality, previous works focused on the second stage. \emph{Time-optimal path-tracking} for robotic manipulators is a long studied problem \cite{shiller1989robot,shin1985minimum,bobrow1985time}. Recently, the convexity of the problem was proven in \cite{verscheure2009time}. The authors in \cite{lipp2014minimum} extended this property to a wider range of systems, and enhanced numerical implementations were presented in \cite{pham2018new,consolini2019optimal}. Regarding \emph{time-optimal path-following}, \cite{verschueren2016time,spedicato2017minimum} leveraged a spatial reformulation of the system dynamics, allowing to compute minimum-time trajectories \emph{around} the predefined path. Exploiting advances in optimization solvers, nonlinear model predictive control (NMPC) methods based on the aforementioned spatial reformulation \cite{arrizabalaga2022towards,kloeser2020nmpc} and contouring control \cite{liniger2015optimization,romero2022model} have shown to approximate time-optimal performance in race-alike scenarios, i.e., \emph{around} a predefined geometric path (the center line of the track).

However, the optimality of all these approaches is upper bounded by the geometric reference predetermined by the path planning stage. In other words, only if \emph{both} the path planning and path tracking/following stages are optimal, will the resultant planned trajectory be optimal, implying that the decoupled nature of these methods jeopardizes the time-optimality of the planned trajectories. This raises the question on how to formulate a motion planning scheme, applicable to any system and constrained environment, that computes the time-optimal trajectory compliant with system dynamics, state/input constraints and spatial bounds.

To answer this question, in this paper we formulate an \emph{offline} motion planning approach capable of \emph{computing dynamically feasible, collision-free and time-optimal trajectories}, by solely relying on a goal location and a geometric representation of the obstacle-free environment. To do so, we present a dual-stage approach, where first a parametric path is computed, and second, a spatially reformulated time minimization is solved. In contrast to the aforementioned decoupled approaches, the parametric path obtained in the first stage is not a geometric reference, but an extension of the environment representation, and consequently, has no effect on the optimality of the time-minimizing problem in the second stage.

The presented scheme is comprised of three main ingredients. First, by leveraging a spatial reformulation, presented in \cite{arrizabalaga2022spatial}, we perform a spatial transformation of the time-based system states to path coordinates. The resultant system dynamics evolve according to a \emph{path parameter} $\xi$ instead of \emph{time} $t$. Second, we leverage Pythagorean Hodograph curves to efficiently and analytically compute the parametric functions required by this spatial reformulation. Third, using the first and second ingredients, the time minimization problem is reduced into a finite horizon spatial problem.

\noindent More specifically, we make the following contributions:
\begin{enumerate}
    \item We extend the applicability of the spatial reformulation introduced in \cite{arrizabalaga2022spatial}, by showing how it allows to conduct a singularity-free spatial transformation of the dynamics for any arbitrary system.
    \item We provide additional insights into the computation of Pythagorean Hodograph splines, allowing us to specify the initial and final orientations of its adapted frame, and as a result, reduce the amount of free coefficients.
    \item We derive a motion planning methodology that (i) is applicable to any system and constrained environment and (ii) directly computes a collision-free and dynamically feasible time-optimal trajectory without relying on a higher level geometric reference.
\end{enumerate}

The remainder of this paper is structured as follows: Section~\ref{sec:problem} introduces the spatially constrained time-optimal motion planning problem. Section~\ref{sec:solution_approach} presents the solution proposed in this paper, by revisiting the aforementioned spatial reformulation, its compatibility with PH curves, and performing a spatial transformation of the time minimization problem. Experimental results are shown in Section~\ref{sec:experiments} before Section~\ref{sec:conclusion} presents the conclusions.

\vspace{2mm}
\noindent\textit{Notation:} We will use $\dot{(\cdot)} = \dv{(\cdot)}{t}$ for time derivatives and $(\cdot)' = \dv{(\cdot)}{\xi}$ for differentiating over path parameter $\xi$. For readability we will employ the abbreviation $ {}_t\xi=\xi(t)$.

\section{PROBLEM STATEMENT}\label{sec:problem}
\noindent We consider continuous time, nonlinear systems of the form
\begin{subequations}\label{eq:nonlinear_sys}
\begin{flalign}
    &\dot{\bm{x}}(t) = f(\bm{x}(t),\bm{u}(t)),\quad \bm{x}(t_0) = \bm{x_0}\,,\label{eq:nonlinear_sys1}\\
    &\bm{y}(t) = h(\bm{x}(t))\label{eq:nonlinear_sys2}\,,
\end{flalign}
\end{subequations}
where $\bm{x}\in\mathcal{X}\subseteq\mathbb{R}^{n_x}$ and $\bm{u}\in\mathcal{U}\subseteq\mathbb{R}^{n_u}$ define state and input constraints. Function $f\,:\,\mathbb{R}^{n_x}\times\mathbb{R}^m\mapsto\mathbb{R}^{n_x}$ refers to the equations of motion of an arbitrary dynamic system,  while the map $h\,:\,\mathbb{R}^{n_x}\mapsto\mathbb{R}^{n_y}$ defines the output of the system $\bm{y}\in\mathbb{R}^{n_y}$. Both $f$ and $h$ functions are assumed to be sufficiently continuously differentiable. The initial state is contained in a subset of the constrained state set, i.e.,  $\bm{x}_0\in\mathcal{X}_0\subseteq\mathcal{X}$.

To ensure that the system remains in the obstacle free space, the geometrically constrained environment is described as
\begin{equation}\label{eq:free_space}
    \Omega = \{g(h(\bm{x})) < 0\,,\quad\forall \bm{x}\in\mathcal{X}\}\, \in\mathbb{R}^{n_y}\,,
\end{equation}
where we also assume the function $g\,:\,\mathbb{R}^{n_y}\mapsto\mathbb{R}^{n_y}$ to be sufficiently often continuously differentiable.

Spatially Constrained Time-Optimal Motion Planning refers to the problem of planning a trajectory that steers system~\eqref{eq:nonlinear_sys} from its initial state $\bm{x_0}$ to a goal output state $\bm{y}_\mathbf{f}\in\mathbb{R}^{n_y}$ in minimum-time, without compromising the system dynamics in~\eqref{eq:nonlinear_sys1}, ensuring the integrity of state and input constraints -- $\bm{x}\in\mathcal{X}$,  $\bm{u}\in\mathcal{U}$ -- and guaranteeing that output~\eqref{eq:nonlinear_sys2} remains within the free space in~\eqref{eq:free_space}, i.e., $\bm{y}\in\Omega$. This is equivalent to solving the following optimal control problem (OCP):

\begin{subequations}\label{eq:minT_cont}
    \begin{flalign}
    &\qquad\qquad\min_{\bm{x}(\cdot),\bm{u}(\cdot)} T = \int_0^{T} dt&
    \end{flalign}
    \vspace{-5mm}
	\begin{alignat}{3}
	\text{s.t.}\quad& \bm{x}(0) = \bm{x_0}\,,\\
	&\dot{\bm{x}} = f(\bm{x}(t),\bm{u}(t)), &\quad&t \in \left[0,T\right]\\
	&\bm{x}(t)\in\mathcal{X}\,,\,\bm{u}(t)\in\mathcal{U}\,,    &\quad&t \in \left[0,T\right]\\
	&\bm{y}(t)\in\Omega\,,    &\quad&t \in \left[0,T\right]\\
	&\bm{y}(T) = \bm{y}_\mathbf{f}\,.
	\end{alignat}

\end{subequations}
As mentioned earlier, \emph{decoupled} approaches separate this problem into a path planning and path tracking/following problem. The computational tractability advantages brought by these methods come at the expense of an inherited suboptimality in the planned trajectories. We deem to close this gap by formulating a method that \emph{directly} solves the minimum-time problem~\eqref{eq:minT_cont}, eliminating the need for the path planning stage, and thus leveraging the entire free space to compute the time-optimal trajectory.

\begin{comment}
More specifically, in this paper we tackle the following motion planning problem:
\noindent\textbf{Spatially Constrained Time-Optimal Motion Planning Problem}
\noindent\textit{Given system~\eqref{eq:nonlinear_sys}, a geometrically constrained environment described in~\eqref{eq:path} and the time minimization problem in~\eqref{eq:minT_cont}, formulate a planner that fulfills the following:
\begin{itemize}
    \item[\textbf{D1}] \textbf{Time-optimality:} The path-following error converges to the $\rho(\xi)$-neighborhood of path $\Gamma$ $\lim_{t\to\infty} ||\bm{e}_\Gamma(t)||^2 \leq \rho({}_t\xi)\,$.
    \item[\textbf{D2}] \textbf{Convergence on path:} The system converges to the end of the path $\lim_{t\to\infty} \xi_f-\xi(t) = 0$.
    \item[\textbf{D3}] \textbf{Constraint satisfaction:} The state and input constraints $\bm{x}(t)\in\mathcal{X}$ and $\bm{u}(t)\in\mathcal{U}$ are fulfilled for all $t\geq0$ .
\end{itemize}}
\end{comment}
\section{SOLUTION APPROACH}\label{sec:solution_approach}
\noindent The motion planning scheme presented in this paper leverages (i) a \emph{spatial transformation of the system dynamics} with respect to (ii) an \emph{arbitrary parametric curve with an associated adapted frame} to perform (iii) a \emph{spatial reformulation of the time-minimizing problem}~\eqref{eq:minT_cont}. These three ingredients are the main building blocks of the proposed methodology. In this section, we present further details on each of them.

\subsection{SPATIAL TRANSFORMATION OF SYSTEM DYNAMICS}

\noindent Let $\Gamma$ refer to a geometric reference and be defined as a path with an associated adapted-frame, whose position and orientation are given by two sufficiently continuous functions, $\bm{\gamma}\,:\,\mathbb{R}\mapsto\mathbb{R}^3$ and $\text{R}\,:\,\mathbb{R}\mapsto\mathbb{R}^{3\times3}$, that depend on path parameter $\xi$:
\begin{equation}\label{eq:path}
    \Gamma = \{\xi \in[\xi_0,\xi_f] \subseteq\mathbb{R}\mapsto\bm{\gamma}(\xi) \in \mathbb{R}^3, \text{R}(\xi) \in \mathbb{R}^{3\times3}\}
\end{equation}
Decomposing the adapted-frame into its components $\text{R}(\xi) = \{\bm{e_1}(\xi),\bm{e_2}(\xi),\bm{e_3}(\xi)\}$ allows to define its angular velocity vector as 
$\bm{\omega}(\xi) = \rchi_1(\xi)\bm{e_1}(\xi) + \rchi_2(\xi)\bm{e_2}(\xi) + \rchi_3(\xi)\bm{e_3}(\xi)$,
where $\rchi(\xi) = \{\rchi_1(\xi),\rchi_2(\xi),\rchi_3(\xi)\}\in\mathbb{R}^3\mapsto\mathbb{R}$ is also sufficiently continuous. 

In \cite{arrizabalaga2022spatial} we show that the equations of motion of the spatial coordinates -- progress along the path $\xi$ and the orthogonal distance to it $\bm{w}=\left[w_1,w_2\right]$ -- associated to a point-mass moving at speed $\bm{v}\in\mathbb{R}^3$ with respect to path $\Gamma$ are given by
\begin{subequations}\label{eq:point_mass_eq}
\begin{gather}
    \dot{\xi}(t) = \frac{\bm{e_1}({}_t\xi)^\intercal \bm{v}(t)}{\sigma({}_t\xi) - \rchi_3({}_t\xi) w_1(t) + \rchi_2({}_t\xi) w_2(t)}\,,\label{eq:xidot}\\
    \dot{w}_1(t) = \bm{e_2}({}_t\xi)^\intercal \bm{v}(t) + \dot{\xi}(t) \rchi_1({}_t\xi) w_2(t)\,,\\
    \dot{w}_2(t) = \bm{e_3}({}_t\xi)^\intercal \bm{v}(t) - \dot{\xi}(t) \rchi_1({}_t\xi) w_1(t)\, ,
\end{gather}
\end{subequations}
where $\sigma(\cdot)$ stands for the parametric speed of path $\Gamma$. For a better understanding of the spatial states, see Fig.~\ref{fig:spatial_coordinates}. These states, alongside their respective equations of motion~\eqref{eq:point_mass_eq}, allow for a projection of the Euclidean coordinates into the geometric path $\Gamma$, resulting in a change of coordinates. Doing so, embeds the geometric properties \emph{around path $\Gamma$} into the system dynamics, and thus, it has become a very popular approach among recent path following methods \cite{arrizabalaga2022towards,spedicato2017minimum,van2016path,kloeser2020nmpc}.

Given that our solution aims to be agnostic from a geometric reference, instead of projecting the Euclidean coordinates on path $\Gamma$, we perform a spatial transformation of the system dynamics in~\eqref{eq:nonlinear_sys}. To do so, we leverage the chain rule as follows:
\begin{gather}\label{eq:spatial_transform1}
    \bm{x}'\coloneqq \dv{\bm{x}}{\xi} = \dv{\bm{x}}{t}\dv{t}{\xi}
\end{gather}
Noticing that $\dv{t}{\xi} = 1/\dot{\xi}$ and assuming that $\dot{\xi}\neq0$, \eqref{eq:spatial_transform1} is simplified into
\begin{gather*}
    \bm{x}'= \frac{1}{\dot{\xi}} f(\bm{x},\bm{u})\,,\quad \forall \dot{\xi}\neq0\,.
\end{gather*}
The resultant spatially transformed equations of motion for the dynamic system~\eqref{eq:nonlinear_sys} are
\begin{subequations} \label{eq:nonlinear_sys_param}
\begin{flalign}
    &\bm{x}'(\xi) = \frac{f(\bm{x}(\xi),\bm{u}(\xi))}{\dot{\xi}(\bm{x}(\xi))},\quad\label{eq:transformed_dynamics} \bm{x}(\xi_0) = \bm{x_0}\,,\\
    &\bm{y}(\xi) = h(\bm{x}(\xi))\,,
\end{flalign}
\end{subequations}
where $\dot{\xi}(\bm{x}(\xi))$ is obtained from~\eqref{eq:xidot}. Comparing the \emph{spatially transformed} system~\eqref{eq:nonlinear_sys_param} with respect to the original system~\eqref{eq:nonlinear_sys}, it becomes apparent that the dynamics evolve with respect to \emph{path parameter $\xi$} instead of \emph{time $t$}. However, in contrast to the aforementioned projection-based methods, the states of~\eqref{eq:nonlinear_sys_param} remain the same as in~\eqref{eq:nonlinear_sys}.

Transforming the system dynamics according to the chain rule~\eqref{eq:spatial_transform1} is a mature technique \cite{shiller1989robot,verscheure2009time}. Nevertheless, to the best of the authors' knowledge, this is the first time that it is applied to the novel spatial reformulation~\eqref{eq:point_mass_eq}. When doing so, the benefits of this reformulation are inherited by the transformed system~\eqref{eq:nonlinear_sys_param}. As opposed to state-of-the-art Frenet-Serret based reformulations \cite{van2016path,verschueren2016time}, \eqref{eq:point_mass_eq} does not take any assumptions in its adapted frame, and as result, it overcomes the two major drawbacks of the Frenet-Serret frame: (i) \emph{singularities} when the curvature vanishes and (ii) an \emph{undesired twist} with respect to its tangent component. 
\begin{comment}
For further details on the advantages of the novel spatial reformulation~\eqref{eq:point_mass_eq}, please refer to \cite{arrizabalaga2022spatial}.
\end{comment}

\subsection{PYTHAGOREAN HODOGRAPH SPLINES}
\noindent For a complete definition of the spatially transformed system~\eqref{eq:nonlinear_sys_param}, the \emph{parametric functions} associated to $\dot{\xi}$ -- the parametric speed $\sigma(\xi)$ and the adapted frame's rotation matrix $\text{R}(\xi)$ and angular velocity $\bm{\omega}({\xi})$ -- need to be expressed in closed form. To this end, we employ Pythagorean Hodograph (PH) curves.
\vspace{1mm}
\subsubsection*{Preliminaries on PH curves}
\hspace{1cm}

\vspace{1mm}
\noindent PH curves are a subset of polynomial curves whose parametric speed is a polynomial of the path parameter $\xi$ \cite{farouki2008pythagorean}. Decomposing the components of $\bm{\gamma}(\xi)$ in~\eqref{eq:path} into $x(\xi),y(\xi),z(\xi)$, the condition for a polynomial to be a PH curve is equivalent to
\begin{gather}\label{eq:ph_cond}
    \sigma^2(\xi) = {x'}^2(\xi)+ {y'}^2(\xi) + {z'}^2(\xi)\,,
\end{gather}
where $\sigma(\xi)$ is a polynomial.  As proven in \cite{dietz1993algebraic}, every term in~\eqref{eq:ph_cond} can be expressed in terms of a \emph{quaternion polynomial} $\bm{Z}(\xi) = u(\xi) + v(\xi)\textbf{i} +g(\xi)\textbf{j} +h(\xi)\textbf{k}$ \footnote{$\{\textbf{i},\textbf{j},\textbf{k}\}$ refers to the $\mathbb{R}^{4}$ standard basis}, whose components $u(\xi),v(\xi),g(\xi),h(\xi)$ are also polynomial functions of the path parameter $\xi$. Consequently, the parametric speed can be reformulated as
\begin{equation}\label{eq:sigma_sq}
    \sigma(\xi)  = u^2(\xi) + v^2(\xi) + g^2(\xi) + h^2(\xi)\,.
\end{equation}
Another notable benefit of PH curves is that they inherit a continuous adapted frame, denoted as \emph{Euler Rodrigues Frame} (ERF), which also exclusively depends on its quaternion polynomial \cite{choi2002euler}.  Its respective rotation matrix is given by
\begin{equation}\label{eq:erf}
    \text{R}(\xi) = \left[\bm{Z}(\xi)\bi\bm{Z}^*(\xi), \bm{Z}(\xi)\bj\bm{Z}^*(\xi), \bm{Z}(\xi)\bz\bm{Z}^*(\xi)\right]\,,
\end{equation}
where $(\cdot)^*$ refers to the quaternion's conjugate. Finally, considering that the components of its angular velocity are expressed as
\begin{equation}\label{eq:angvel_comp}
    \rchi(\xi) = \{\bm{e_2}'(\xi)\,\bm{e_3}(\xi),\bm{e_3}'(\xi)\,\bm{e_1}(\xi),\bm{e_1}'(\xi)\,\bm{e_2}(\xi)\}\,,
\end{equation}
the adapted frame's angular velocity  $\bm{\omega}(\xi)$ is likewise exclusively reliant on the quaternion polynomial. 

From~\eqref{eq:sigma_sq},~\eqref{eq:erf} and~\eqref{eq:angvel_comp}, it can be stated that all \emph{parametric functions} -- $\sigma(\xi),\text{R}(\xi),\bm{\omega}(\xi)$ -- solely depend on the quaternion polynomial $\bm{Z}(\xi)$, and consequently, they are fully defined by the coefficients of the underlying polynomial functions $u(\xi),v(\xi),g(\xi),h(\xi)$. For convenience we will refer to these as \emph{PH coefficients}, $\bm{\zeta}\in\mathbb{R}^{n_\zeta}$. Finally, notice that eq.~\eqref{eq:sigma_sq} entails that a quaternion hodograph of degree $n$ corresponds to a curve of degree $2n+1$, while relying just on $4(n+1)$ PH coefficients. This implies that all three parametric functions can be efficiently encoded into a handful of coefficients.

\begin{figure}
	\centering
	\includegraphics[width=\linewidth]{figs/spatial_coordinates_ph_new.pdf}
	\caption{Spatial projection of the three-dimensional Euclidean coordinates, represented by the pink dot, onto a geometric path $\Gamma$ with an associated adapted-frame $\text{R}(\xi) = \{\bm{e_1}(\xi),\bm{e_2}(\xi),\bm{e_3}(\xi)\}$. The distance to the closest point on the path $\bm{\gamma}(\xi)$ is decomposed into the transverse coordinates $\bm{w} = \left[w_1,w_2\right]$.}\label{fig:spatial_coordinates}
\end{figure}


\vspace{1mm}
\subsubsection*{Collision free PH splines}
\hspace{1cm}

\vspace{1mm}

\noindent To ensure planning capabilities within highly non-convex and complex environments, we concatenate multiple PH curves into \emph{PH splines}. When doing so, it is required to ensure that the parametric functions -- $\sigma(\xi),\text{R}(\xi),\bm{\omega}(\xi)$ -- remain sufficiently often continuously differentiable. Out of all three, the angular velocity is the one with a lowest degree of $n-1$. For compliance with second order optimization methods, the parametric functions need to be $C^2$, equivalent to a minimum order of $n=4$ or a curve of degree $9$, also referred as \emph{nonics}.

Aside from being continuous, the PH spline also needs to be collision-free. For this purpose, without loss of generality, we perform a convex decomposition of the free space, i.e., $\Omega = \cup_{k=1}^m\,\mathcal{P}_{k}$, where $m$ refers to the number of spline segments. Consequently, we can guarantee the integrity of the spatial bounds by leveraging the Convex Hull property and ensuring that the control points of each segment of the spline stay encompassed inside the associated convex set. These control points can readily be obtained from a function $q\,:\,\mathbb{R}^3\times\mathbb{R}^{n_\zeta}\mapsto\mathbb{R}^{2m(n+1)}$ that takes the starting location of the spline $\bm{p_0}\in\mathbb{R}^3$ and the PH coefficients as inputs. For the respective analytical expressions check \cite{farouki2008pythagorean} (for cubics to septics) or \cite{arrizabalaga2022spatial} (eq. 17, for nonics).
\vspace{1mm}
\subsubsection*{Optimization of PH coefficients}
\hspace{1cm}

\vspace{1mm}
\noindent As mentioned earlier, a PH curve whose quaternion polynomial is of degree $n$ is fully defined by $4(n+1)$ PH coefficients. Therefore, a PH spline with $m$ sections depends on $n_\zeta = 4m(n+1)$ coefficients. However, imposing the aforementioned continuity condition implies that only a subset of these are free variables. Moreover, allocating a starting and ending pose to the PH spline further reduces the amount of independent coefficients. 

As an illustrative example, we focus on PH nonic curves $n=4$, i.e., the minimum degree ensuring that the parametric functions are $C^2$. Every section contains $4(n+1)=20$ coefficients, but from the second section onward, due to continuity only $4$ per segment are free. For the analytical expressions that relate the coefficients between segments, see eq. 16 in \cite{arrizabalaga2022spatial}. Furthermore, from~\eqref{eq:erf} the initial and final orientations of the adapted frame -- $\text{R}(\xi_0), \text{R}(\xi_f)$ -- relate to their respective unit-quaternion representation, and thus, are equivalent to the first and last tuple of PH coefficients. Finally, forcing the PH spline to reach an end-point is the same as constraining the last control point of the last segment. Leveraging a Hopf map projection, this property allows to reduce the last tuple of the penultimate segment into a single coefficient. Putting all together, the resultant the PH coefficients are diminished into its minimum form. As a result, a PH nonic spline with $m$ sections, $C^2$ parametric functions and a predetermined end pose, is fully defined by $4m+5$ PH coefficients. Fig.~\ref{fig:ph_coefficients} provides a schematic overview of the independent coefficients for this particular example.

By solving an optimization problem on the remaining free coefficients, we can tailor the shape of the PH spline according to a desired criterion. When doing so, a common practice is to minimize the \emph{energy} of the spline, resulting in a twist-free and low-curvature geometric path \cite{arrizabalaga2022spatial, albrecht2020spatial}. Denoting the \emph{compact representation} of the PH coefficients as $\bm{\mathcal{Z}}\in\mathbb{R}^{n_z}$ with $n_z<n_\zeta\,$, the optimization problem that computes a collision-free PH spline starting from $\bm{p_0}$ with $m$ segments for a generic function criterion $f_\text{PH}$ is

\begin{subequations}\label{eq:ph_opt}
    \begin{flalign}
    &\qquad\qquad\min_{\bm{\mathcal{Z}}} \int_{\xi_0}^{\xi_f} f_\text{PH}(\bm{p_0},\bm{\mathcal{Z}})\, d\xi&
    \end{flalign}
    \vspace{-5mm}
	\begin{alignat}{3}
	\text{s.t.}\quad& \tilde{q}(\bm{p}_{\bm{0}},\bm{\mathcal{Z}})_k\in\mathcal{P}_k\,,    &\quad&k = 1,\, \cdots\,, m\,, \label{eq:spatial_constraint}
	\end{alignat}
\end{subequations}
where $\tilde{q}\,:\,\mathbb{R}^3\times\mathbb{R}^{n_z}\mapsto\mathbb{R}^{2m(n+1)}$ maps the starting point and the compact coefficient representation to the control points of the PH spline. Constraint~\eqref{eq:spatial_constraint} obliges the control points associated to a given section $k$ to be enclosed within the convex set $\mathcal{P}_k$. Extending this to all $m$ sections ensures that the entire PH spline lies within the free space $\Omega$.
\begin{figure}
	\centering
	\includegraphics[width=\linewidth]{figs/ph_coefficients.pdf}
	\caption{Illustrative representation of PH coefficients for PH splines of degree $9$ in four different case-studies. Each case relates to a column, i.e., the spline in the first column only has $1$ segment, while the one at the fourth column has $4$ segments. For a PH curve of degree $9$ ($n=4$), each polynomial $u,v,g,h$ relates to $n+1 = 5$ coefficients, and as a result, the total amount of coefficients at each section of the PH spline is $4(n+1)=20$, depicted by dashed squares. However, when imposing initial and final adapted frame orientations, continuity conditions and an ending position, the amount of free coefficients is reduced. These are shown in white, while the rest are colored according to the reason they are not free.}\label{fig:ph_coefficients}
\end{figure}
\subsection{TIME MINIMIZATION: A SPATIAL APPROACH}
\noindent The PH coefficients $\bm{\mathcal{Z}}^*$ obtained from solving problem~\eqref{eq:ph_opt} explicitly describe the parametric coefficients. and therefore, allow to rewrite the equation of motion of the spatially transformed system~\eqref{eq:transformed_dynamics} as
\begin{equation} \label{eq:nonlinear_sys_param_ph}
    \bm{x}'(\xi,\bm{\mathcal{Z}}^*) = \frac{f(\bm{x}(\xi),\bm{u}(\xi))}{\dot{\xi}(\bm{x}(\xi),\bm{\mathcal{Z}}^*)}\,.
\end{equation}
This equation represents the spatially transformed dynamic model of a system whose temporal dynamics are given by $f$, with respect to a PH spline defined by PH coefficients $\bm{\mathcal{Z}}^*$. In contrast to~\eqref{eq:nonlinear_sys_param}, the system in~\eqref{eq:nonlinear_sys_param_ph} accounts for a method to compute the underlying parametric functions -- based on PH splines --, and thus, it is fully defined. 

Revisiting the fact that  $\dv{t}{\xi} = 1/\dot{\xi}$ and leveraging the spatially transformed system dynamics in~\eqref{eq:nonlinear_sys_param_ph}, the time-minimizing problem in~\eqref{eq:minT_cont} can be reformulated as
\begin{subequations}\label{eq:minT_par}
    \begin{flalign}
    &\qquad\qquad\min_{\bm{x}(\cdot),\bm{u}(\cdot)} T = \int_{\xi_0}^{\xi_f}\frac{1}{\dot{\xi}(\bm{x}(\xi),\bm{\mathcal{Z}}^*)}\, d\xi&
    \end{flalign}
    \vspace{-5mm}
	\begin{alignat}{3}
	\text{s.t.}\quad& \bm{x}(\xi_0) = \bm{x_0}\,,\\
	&\bm{x}' = \frac{f(\bm{x}(\xi),\bm{u}(\xi))}{\dot{\xi}(\bm{x}(\xi),\bm{\mathcal{Z}}^*)}, &\quad&\xi \in \left[\xi_0,\xi_f\right]\\
	&\bm{x}(\xi)\in\mathcal{X}\,,\,\bm{u}(\xi)\in\mathcal{U}\,,    &\quad&\xi \in \left[\xi_0,\xi_f\right]\\
	&\bm{y}(\xi)\in \mathcal{P}(\xi)\,,    &\quad&\xi \in \left[\xi_0,\xi_f\right]\\
	&\bm{y}(\xi_f) = \bm{y}_\mathbf{f}\,,
 \label{eq:mpc_spatial_cond_cont}
	\end{alignat}
\end{subequations}
\begin{figure*}[t]
\centering
\includegraphics[width=\textwidth]{figs/car_results_lr.pdf}
\caption{Planar application of the proposed motion planning approach to a 1:43 scale autonomous car. The blue polygons represent the sets resulting from the convex decomposition of the track. \emph{Left:} The minimum-energy PH spline is depicted according to its adapted frame. The control points associated to every section of the spline are featured as black dots and constrained to lie within one of the $17$ convex sets. \emph{Center:} The planned path is given by the colored line. The color mapping relates to the car's longitudinal velocity. \emph{Right:} The first two rows represent the longitudinal and lateral accelerations, while the third shows the evolution of path parameter's speed profile with respect to time.}\label{fig:car_results}
\vspace{-5mm}
\end{figure*}
where $\mathcal{P}(\hat{\xi})$ refers to the convex set associated to the segment of the PH spline when $\xi=\hat{\xi}$. The resultant OCP~\eqref{eq:minT_par} is a finite horizon problem, and unlike the original problem~\eqref{eq:minT_cont}, the integration interval on which we solve the optimization is independent of the decision variables. Similar spatial reformulations of the time minimization problem have already been conducted previously \cite{verschueren2016time, spedicato2017minimum}. However, these are system-specific and limited to Frenet-Serret based reformulations. Both shortcomings are solved by \eqref{eq:minT_par}, which is compatible with any system whose dynamics are given by $f$ and applicable to a geometric path whose adapted frame can be tailored by PH coefficients $\bm{\mathcal{Z}}^*$.

\subsection{OVERVIEW OF THE METHOD}
\noindent A summary of the proposed motion planning scheme is attached in Algorithm~\ref{alg:algorithm1}. The key difference with respect to state-of-the art approaches is that our method is agnostic from a geometric reference, allowing full exploitation of the free space, and as a result, ensuring the time-optimality of the solution. Because both stages account for the same spatial constraints, the solution of the time minimization problem~\eqref{eq:minT_par} is independent from the PH coefficients obtained in~\eqref{eq:ph_opt}. This emphasizes the idea that \emph{the PH spline is not a geometric reference, but an extension of the environment's representation}.

\begin{algorithm}[h]
\caption{\textit{Spatially Constrained Time-Optimal Motion Planning} (SCTOMP): Given state $\bm{x_0}$, goal $\bm{y}_\mathbf{f}$ and environment $\Omega$, find the time-optimal set of states and inputs $\bm{x}^*,\bm{u}^*$}\label{alg:algorithm1}
\begin{algorithmic}[1]
\Function{SCTOMP}{$\bm{x}$, $\bm{y}_\mathbf{f}$, $\Omega$}
\State $\bm{\mathcal{P}} =\mathcal{P}_1,...\,,\mathcal{P}_m \gets \textsc{Convex Decomposition}(\Omega)$
\State $\bm{p_0}, \bm{p}_\mathbf{f} \gets \textsc{Get Start And End}(\bm{x},\bm{y}_\mathbf{f})$
\State $\bm{\mathcal{Z}}^* \gets \textsc{Compute PH Spline}(\bm{p_0},\bm{p}_\mathbf{f},\bm{\mathcal{P}})$~\eqref{eq:ph_opt}
\State $\bm{x}^*,\bm{u}^* \gets \textsc{Time Minimization}(\bm{x},\bm{y}_\mathbf{f},\bm{\mathcal{Z}}^*)$~\eqref{eq:minT_par}
\State\Return $\bm{x}^*,\bm{u}^*$
\EndFunction
\end{algorithmic}
\end{algorithm}

\section{EXPERIMENTS}\label{sec:experiments}
\begin{figure*}[t]
\centering
\includegraphics[width=\textwidth]{figs/quad_results_lr.pdf}
\caption{Spatial application of the proposed motion planning approach to a quadrotor. The blue polyhedrons represent the convex sets resulting from the decomposition of an illustrative free space. They are numbered according to the associated PH spline segments. \emph{Left:} Starting from the top in clock-wise direction, isometric-view, side-view and top-view. The PH splines related to minimum arc-length, energy and twist are depicted by green, violet and dark orange lines, respectively. The quadrotor's minimum-time trajectory associated to the PH spline with minimum energy is given by the colored line. The color mapping relates to the norm of the quadrotor's velocity, while its orientation is represented by the attached frame. \emph{Right:} At the top, an isometric view of the minimum energy PH spline with its adapted frame and control points. At the bottom, the control commands and path parameter's velocity profile for the minimum time trajectories obtained for all three PH splines. The four divisions in the horizontal axis split the data according to the spline sections.}\label{fig:results}
\vspace{-1mm}
\end{figure*}
\begin{table*}[t!]
   
    \renewcommand{\arraystretch}{1.25}
	\centering
	\caption{States, inputs and constraints for the planar (car) and spatial (quadrotor) systems.} \label{tab:states_inputs_constraints}
	\begin{tabular}{|c||c|c|c|}
		\hline
		System & States $\bm{x}$ & Inputs $\bm{u}$ & Constraints \\
		\hline
		Car & $\{\bm{p},\psi,v,D,\delta\}$ & $\{\dot{D},\dot{\delta}\}$ & $D\in[-1,1]$\,,\;$\delta\in[-0.4,0.4]$\,,\;$\dot{D}\in[-10,10]$\,,\;$\dot{\delta}\in[-2,2]$\,,\;$\{a_\parallel, a_\perp\}\in[-4,4]$\\
		\hline
		Quadrotor  & $\{\bm{p},\bm{v},\bm{q}\}$ & $\{f_c,\bm{\omega}\}$& $f_c\in[0,27.52]$\,,\;$\{\omega_x,\omega_y\}\in[-15,15]$\,,\;$\omega_z\in[-0.3,0.3]$ \\
		\hline
	\end{tabular}
\vspace{-4mm}
\end{table*}
\noindent To evaluate our approach, we divide the experimental analysis into two parts. First, we benchmark our methodology by analyzing its performance in a well-known planar scenario. Second, to demonstrate that the proposed scheme remains time-optimal regardless of the underlying PH spline, we focus on a spatial case.

\vspace{1mm}
\noindent \emph{Numerical implementation:} Both OCPs~\eqref{eq:ph_opt} and~\eqref{eq:minT_par} are modeled in CasADi \cite{andersson2019casadi} and solved by IPOPT \cite{wachter2006implementation} with MA27 \cite{HSL} as the linear solver back-end. While OCP~\eqref{eq:ph_opt} can be readily fed to the optimization routine, problem~\eqref{eq:minT_par} is approximated by a multiple-shooting approach \cite{bock1984multiple}, where the optimization horizon $\xi_f-\xi_0$ is discretized into sections with constant decision variables. The underlying integration routines are approximated by a 4th-order Runge-Kutta method.

\subsection{BENCHMARKING TIME-OPTIMALITY}
\noindent We first assess the performance of our method in a state-of-the-art race track for 1:43 scale autonomous cars. Given that this case-study has been the research focus of previous work \cite{kloeser2020nmpc,verschueren2014towards}, its time-optimality is well-understood.

It is worth highlighting that our method shows its full potential in unstructured environments, where imposing a geometric reference might be detrimental for planning time-optimal trajectories. However, race-alike scenarios, such as the one at hand, consist of well-structured environments in which the progress along the track and its boundaries are perfectly referenced by the centerline, and thus are better suited to path following methods, such as \cite{arrizabalaga2022spatial,romero2022model,kloeser2020nmpc}. Nevertheless, the proposed case-study allows for (i) performing an \emph{approximated benchmark} on time-optimality and (ii) showing its applicability to constrained planar systems.

For this purpose, we perform a convex decomposition of the aforementioned race track, first introduced in \cite{verschueren2014towards} and further studied in \cite{kloeser2020nmpc}. In a similar way as done in these works, we employ the dynamic bicycle model with states $\bm{x}=\left[p_x,p_y,\psi,v,D,\delta\right]\in\mathbb{R}^6$ and inputs $\bm{u}=\left[\dot{D},\dot{\delta}\right]\in\mathbb{R}^2$, where $\{p_x,p_y,\psi,v,D,\delta\}\in\mathbb{R}$, refer to the position, yaw, longitudinal velocity, throttle and steering angle. For the respective equations of motion and model coefficients, please refer to eq. 3 in \cite{kloeser2020nmpc}. To guarantee the validity of the model's approximation, we impose constraints on the throttle, steering angle, their respective change rates, as well as the longitudinal and lateral accelerations $\{a_\parallel,a_\perp\}\in\mathbb{R}$. The numerical values associated to these constraints can be found in Table~\ref{tab:states_inputs_constraints}.

As suggested in Algorithm~\ref{alg:algorithm1}, before solving the time-minimizing problem, we find a PH spline that lies within the $17$ convex sets resulting from the earlier mentioned decomposition. By solving~\eqref{eq:ph_opt} for minimum-energy, we obtain the spline, featured in the left column of Fig~\ref{fig:car_results}. Given the planar nature of the problem --there is no twist--, and therefor, the obtained PH spline relates to the path with minimum curvature, as suggested by the depicted adapted frame. Using the PH coefficients of this spline, we solve the time minimization problem~\eqref{eq:minT_par} for the bicycle model and its respective constraints. The resulting trajectory is shown in the center column of the same figure. As common in racing scenarios, it enters a corner from its inner side and exists from the outside. Moreover, the longitudinal and angular accelerations attached in the right column of Fig.~\ref{fig:car_results} show that the car's actuation is fully exploited by always remaining on its physical limit. The obtained trajectory results in a lap time of \SI{4.9}{\second}, roughly equivalent to the theoretical minimum of \SI{4.85}{\second} stated in \cite{kloeser2020nmpc}. This difference is attributed to variations in initial and final conditions -- \cite{kloeser2020nmpc} measures the duration of a flying lap --, as well as the fact that the decomposed space is an approximation of the original track. Therefore, the benchmark in \cite{kloeser2020nmpc} is an unrealistic lower bound to our problem.

This experiment suggests that for a given decomposed environment and constrained system, the presented motion planning scheme is able to compute dynamically feasible time-optimal trajectories. In contrast to other works, our method is agnostic to the track's center line, i.e., it only relies on the decomposed representation, making it applicable to any geometrically constrained environment.

\subsection{ON THE INFLUENCE OF PH SPLINES}
\noindent After having analyzed the method's time-optimality, in this section we study the sensitivity of the solution with respect to the PH spline. As specified in~\eqref{eq:ph_opt}, the cost function $f_\text{PH}$ upon which the spline is computed can be tailored to any criterion. To put it another way, any candidate that lies within the decomposed space is eligible to be embedded in the time optimization problem~\eqref{eq:minT_par}. Nonetheless, because the decomposed free space remains fixed, the time-optimal solution should be invariant regardless of the PH spline used.

To tackle this question, we focus on a spatial case-study introduced in \cite{arrizabalaga2022spatial}, consisting of a non-convex geometry decomposed into four polyhedron. As an exemplary system we pick a quadrotor with collective thrust and body rates as inputs. Selecting single rotor thrust commands would allow for a bigger exploitation of the system capacities, leading to lower navigation times. However, the aim of this study is to study the invariance of the solution with respect to the PH splines, and thus, we remain in a higher level. In fact, this is the preferred control choice for professional human pilots and recently has been benchmarked as the most successful input modality for training policies on quadrotors \cite{kaufmann2022benchmark}.

The states and inputs of the quadrotor are denoted as $\bm{x}=\left[\bm{p},\bm{v},\bm{q}\right]\in\mathbb{R}^{10}$, $\bm{u}=\left[f_c,\bm{\omega}\right]\in\mathbb{R}^4$, where $\{\bm{p},\bm{v}, \bm{\omega}\}\in\mathbb{R}^3$ refer to the position, velocity and body rates, $\bm{q}\in\mathbb{R}^4$ is the unit quaternion and $f_c\in\mathbb{R}$ is the collective thrust. The equations of motion of this system can be found in \cite{arrizabalaga2022towards} and for the specific coefficients we use the same racing quadrotor as in \cite{foehn2021time}. To ensure the integrity of the model, the collective thrust and the angular velocity are bounded to the values in Table~\ref{tab:states_inputs_constraints}.

To study the impact of PH splines, we compute three different versions, according to three distinct criteria: arc-length $L$, energy $E$ (twist and curvature) and twist $E_{\rchi_1}$ (without curvature). The characteristics of the resultant splines are given in the second and third columns of Table~\ref{tab:ph_spline_diff} and depicted in the left side of Fig.~\ref{fig:results}. Furthermore, an isometric view of the adapted frame and control points associated to the spline with minimum energy is shown in the top right. The smoothness of this frame, i.e., absence of undesired twist, singularities and discontinuities, is an exemplary indicator on how PH curve based parameterizations outperform Frenet-Serret ones. Comparing the PH splines against each other, the one with minimum arc-length is the shortest -- resembling a piecewise linear reference --, while the spline with minimum energy is more rounded, making the latter exceedingly lengthy and curvy. Their differences on shape and size allow for challenging our methodology's robustness with respect to PH spline variations.

Solving the time minimization problem~\eqref{eq:minT_par} for all three splines results in three different trajectories, whose input commands are shown at the lower right part of Fig.~\ref{fig:results}. In all instances, the collective thrust remains at its maximum throughout the entire trajectory, and at least one of the body rate components is saturated, suggesting that the quadrotor is operating at its limit. The trajectory related to the minimum energy case can be visualized in the right side of Fig.~\ref{fig:results}. In a similar way to the preceding planar study, the quadrotor fully exploits the spatial bounds by tangentially touching the apex of the curves.

As indicated by the navigation times attached to the fourth column of Table~\ref{tab:ph_spline_diff}, the first two cases result in an equivalent time-optimal solution of \SI{1.703}{\second}, while the latter requires \SI{20}{\milli\second} longer. We deem this difference to two numerical, i.e., implementation-related, reasons. First, using a multiple shooting approach to approximate OCP~\eqref{eq:minT_par} while keeping the discretization constant reduces the number of control actions as the arc-length of the PH spline increases. Second, given the non-convexity of the problem at hand, relying on a \emph{local} solver hinders converging to the \emph{global} optimum. 

From this experiment we learn that (i) the proposed motion planning scheme computes time-optimal solutions regardless of the underlying PH spline -- demonstrated by the equivalence between the navigation times for minimum arc-length and energy PH splines --, but (ii) rigorous optimality guarantees are highly sensitive to the underlying numerical implementation.  Moreover, the resemblance of all three solutions proves how the underlying PH spline is not a geometric reference, but an extended representation of the environment's geometry.

\begin{table}[t!]
   
    \renewcommand{\arraystretch}{1.25}
	\centering
	\caption{A comparison of the three cases employed for analyzing the sensitivity of the time-optimal solution with respect to PH spline variations. The features of the resultant splines -- total energy and arc-length -- obtained from~\eqref{eq:ph_opt} are given in the second and third column, while the respective navigation times -- computed from~\eqref{eq:minT_par} -- are shown in the fourth column. All three PH splines and the trajectory associated to the second case are shown in Fig.~\ref{fig:results}.} \label{tab:ph_spline_diff}
	\begin{tabular}{|c||c|c|c|}
		\hline
		Design criterion $f_\text{PH}$ & Energy & Arc-length [m] & Time [s] \\
		\hline
		\textcolor{greeN}{Arc-length $L$}  & 189.20 & 10.96 & 1.703 \\
		\hline
		\textcolor{violet}{Energy $E$} & 7.63 & 11.98 & 1.703\\
		\hline
		\textcolor{peru}{Twist $E_{\rchi_1}$} & 20.02 & 13.41 & 1.733 \\
		\hline
	\end{tabular}
\vspace{2mm}
\end{table}

\section{CONCLUSION}\label{sec:conclusion}
\noindent In this work, we presented a motion planning approach capable of computing time-optimal trajectories in spatially constrained environments. As opposed to state-of-the-art methods, we directly solve the geometrically constrained time minimization problem, without relying on collision-free geometric references. As a consequence, the time-optimal trajectories computed by our method, not only exploit the system's actuation, but also the available free space. For this purpose, we rely on spatial reformulation that allows for performing a singularity-free spatial transformation of the system dynamics, as well as the time minimization problem. To compute the underlying parametric functions required by this reformulation, we leverage Pythagorean Hodograph splines. This results in a dual-stage scheme, where the initially computed spline coefficients are fed into the spatially reformulated time minimization problem. Similar timings with respect to a well-known planar benchmark account for the method's time-optimal capabilities. Moreover, being solely dependent on the environment broadens its applicability beyond case-specific state-of-the-art solutions. Further experiments on a spatial case show that the solution's approximated time-optimality is agnostic from the underlying spline. Lastly, the versatility of the systems  -- a planar bicycle model and a spatial quadrotor -- and differences in the scale of the environments upon which the experiments were conducted emphasize the motion planner's generality and applicability to a wide range of systems and scenarios.


\newpage
\newcommand{\BIBdecl}{\setlength{\itemsep}{0.45 em}}
\bibliographystyle{IEEEtran}
