# Beta quantile regression for Robust estimation of uncertainty in the
presence of outliers

Author 1  Author 211footnotemark: 1 Author 3

###### Abstract

Quantile Regression (QR) can be used to estimate aleatoric uncertainty in deep
neural networks and can generate prediction intervals. Quantifying uncertainty
is particularly important in critical applications such as clinical diagnosis,
where a realistic assessment of uncertainty is essential in determining
disease status and planning the appropriate treatment. The most common
application of quantile regression models is in cases where the parametric
likelihood cannot be specified. Although quantile regression is quite robust
to outlier response observations, it can be sensitive to outlier covariate
observations (features). Outlier features can compromise the performance of
deep learning regression problems such as style translation, image
reconstruction, and deep anomaly detection, potentially leading to misleading
conclusions. To address this problem, we propose a robust solution for
quantile regression that incorporates concepts from robust divergence. We
compare the performance of our proposed method with (i) least trimmed quantile
regression and (ii) robust regression based on the regularization of case-
specific parameters in a simple real dataset in the presence of outlier. These
methods have not been applied in a deep learning framework. We also
demonstrate the applicability of the proposed method by applying it to a
medical imaging translation task using diffusion models.

Index Terms—  Quantile regression, Diffusion models, Robust divergence

##  1 Introduction

Quantile regression offers an alternative to mean regression in various
applications where accurate predictions and their associated reliability are
crucial. For instance, in clinical diagnosis, a realistic assessment of
prediction uncertainty is essential for determining disease status and
planning appropriate treatment. In the context of deep learning, two types of
uncertainties are encountered: aleatoric and epistemic. Aleatoric uncertainty
arises from the inherent stochasticity of the data, while epistemic
uncertainty — often referred to as model uncertainty — is due to limitations
in the model itself. It’s worth noting that an infinite amount of training
data would not reduce aleatoric uncertainty, although it could mitigate
epistemic uncertainty. A multitude of methods exist for estimating these
uncertainties, including Gaussian process regression, uncertainty-aware neural
networks, Bayesian neural networks, and ensemble methods[1, 2, 3].

Recent studies have proposed to use conditional quantile regression to
estimate aleatoric uncertainty in neural networks [2, 4, 5, 6, 7] and showed
that it can compute well-calibrated intervals. The most common application of
quantile regression models is in cases where parametric likelihood cannot be
specified [8]. Similar to the classical regression analysis which estimates
the conditional mean, the $\alpha$-th quantile regression $(0<\alpha<1)$ seeks
a solution to the following minimization problem [8]:

$\displaystyle\operatorname*{arg\,min}\limits_{\theta}\sum_{i}\rho_{\alpha}(y_{i}-f_{\theta}(x_{i})),$
(1)

where $x_{i}$ are the inputs, $y_{i}$ are the responses, $f$ is the model
paramaterized by $\theta$, and $\rho_{\alpha}$ is the _check function_ or
_pinball loss_ [8] defined as:

$\rho_{\alpha}(y_{i}-f_{\theta}(x_{i}))=\begin{cases}(y_{i}-f_{\theta}(x_{i}))\alpha,&\text{if
}y_{i}\geq f_{\theta}(x_{i})\\\ (f_{\theta}(x_{i})-y_{i})(1-\alpha),&\text{if
}y_{i}<f_{\theta}(x_{i})\end{cases}$

It has been shown that minimization of the loss function in (1) is equivalent
to maximizing the likelihood function formed by combining independently
distributed asymmetric Laplace densities [8],

$\displaystyle\operatorname*{arg\,max}\limits_{\theta}L(\theta)=\frac{\alpha(1-\alpha)}{\sigma}\exp\left\\{\frac{-\sum_{i}\rho_{\alpha}(y_{i}-f_{\theta}(x_{i}))}{\sigma}\right\\}.$

where $\alpha$ is the quantile and $\sigma$ is the scale parameter.

Recently, quantile regression has been employed for uncertainty estimation in
regression tasks such as image translation [9] and anomaly detection [7] in
medical imaging. In these domains, obtaining a reliable uncertainty estimate
is of critical importance. Compared to alternative methods for uncertainty
estimation, such as sampling using generative models [7] or Bayesian
uncertainty estimation, quantile regression offers computational efficiency
and speed, and does not require sampling.

Statistical machine learning models that involve maximizing likelihood are
particularly sensitive to outliers [10]. Although quantile regression is quite
robust to outlying response observations, it can be sensitive to outlier
covariate observations (features). It has been shown that perturbing a single
($x_{i}$, $y_{i}$) data point in an arbitrary manner can force all quantile
regression hyperplanes to intersect at the perturbed point [11]. Despite that,
it’s important to highlight that only a limited number of papers have explored
the robustness of quantile regression in the context of covariate
observations, particularly within deep learning frameworks.

We outline our contributions in this paper as follows: (i) We propose a robust
quantile regression approach that leverages concepts from robust divergence.
(ii) We compare the performance of our proposed method, particularly in the
presence of outliers, to existing techniques such as Least Trimmed Quantile
Regression [11], which serves as the only available baseline, and robust
regression methods that rely on the regularization of case-specific
parameters, in both a simple dataset and a simulated dataset. (iii) Finally,
to illustrate the practical utility of the proposed method, we apply it to a
medical imaging translation task, employing state-of-the-art diffusion models.

##  2 Method

We start by briefly explaining the formulation of least-trimmed quantile
regression [11] and robust regression based on the regularization of case-
specific parameters.

###  2.1 Least Trim Quantile Regression (TQR)

The objective function for TQR is defined as:

$\displaystyle\operatorname*{arg\,min}\limits_{\theta}\sum_{I_{C}}\rho_{\alpha}(y_{i}-f_{\theta}(x_{i}))$
(2)

Where $I_{C}$ is the subset of samples with C samples from the training
dataset that generates the smallest error. The optimization is similar to
quantile regression with an additional iterative process. After initializing
with C random samples, at each iteration, the samples with the smallest error
are chosen for training in the next iteration and the process is repeated
until there are not any significant changes in loss value comparing to that of
the previous iteration. We utilized TQR within a gradient descent optimization
framework, where we used only the subset of the batch with the lowest error
for backpropagation.

###  2.2 Robust regression based on regularization of case-specific parameters
(RCP)

She and Owen [12] proposed a robust regression method using the case-specific
indicators in a mean shift model with the regularization method. By
generalizing their method to quantile regression, the final loss can be
simplified to:

$\displaystyle\operatorname*{arg\,min}\limits_{\theta}\sum_{i}\rho_{\alpha}(y_{i}-f_{\theta}(x_{i})-\gamma_{i})+\lambda\sum_{i}|\gamma_{i}|$
(3)

This optimization can be solved using an alternative approach and soft margin
thresholding. RCP can be used for any likelihood based model.

###  2.3 $\beta$-quantile regression ($\beta$-QR)

For parameter estimation, maximizing the likelihood is equivalent to
minimizing the KL-divergence between the empirical distribution of the input
and statistical model $q(\phi)$. Similarly a robust $\beta$-loss ($L_{\beta}$)
can be derived by replacing the KL-divergence with the $\beta$-divergence
$D_{\beta}$ [13, 14, 15].

$\displaystyle D_{\beta}(f(x)||g(x))$
$\displaystyle=\frac{1}{\beta}\int(f(x)^{\beta}g(x)^{\beta})f(x)dx$
$\displaystyle-\frac{1}{\beta+1}\int(f(x)^{\beta+1}g(x)^{\beta+1})dx$

$\displaystyle L_{\beta}=\frac{1}{N}\sum\frac{\exp(\beta
l(x_{i},q(\phi)))-1}{\beta}+\frac{1}{\beta+1}\int q(\phi)^{\beta+1}$

where $l(x_{i},q(\phi))$ denotes the log-likelihood of observation $x_{i}$.
This loss assigns a weight to each observation based on the likelihood’s
magnitude, mitigating the influence of outliers on model training [13]. In the
case of quantile regression, the loss can be simplified to:

$\displaystyle
L_{\beta\alpha}=\frac{1}{N}\sum\frac{\exp(-\beta\rho_{\alpha}((y_{i}-f_{\theta}(x_{i}))/\sigma)-1}{\beta}$
(4)

The hyperparameter $\sigma$ can be assumed to be 1 for simplicity. This loss
can be interpreted as an M-estimate. The hyperparameter $\beta$ specifies the
degree of robustness.

###  2.4 Quantile regression for diffusion models for regression tasks

Diffusion probabilistic models [16] are primarily composed of two essential
processes: a forward process that gradually adds Gaussian noise to a data
sample, and a reverse process that transforms Gaussian noise to an empirical
data distribution through a gradual denoising process. Conditional diffusion
models [17] incorporate input samples to condition the denoising process.
Image translation problems can be modeled as conditional diffusion models,
represented as: $p(y|x)$, where $y$ is the target image and $x$ is the input
conditioning image. In this paper, we deal with image translation problems
where the input images $x$ are T1-weighted brain MRI images and the targets
$y$ are the corresponding T2-weighted images. The diffusion model
$f_{\theta}(x)$ is trained to recover T2-weighted images $y$ from Gaussian
noise $\epsilon\sim\mathcal{N}(0,I)$ conditioned on the input T1-weighted
images $x$. For the details of the diffusion and conditional diffusion models,
we refer to multiple works that provide a complete treatment of the
mathematical formulations [18, 19, 17, 16, 20]. Instead of minimizing the mean
squared error loss between the targets $y$ and the estimates $f_{\theta}(x)$
that yields a mean regression problem, the minimization problem in (1) is
adopted to predict the $\alpha$ quantiles of the target images. We show that
in the presence of outliers in the training set, replacing the loss function
in (1) with the proposed loss function in (4) yields a model that is minimally
affected by the outliers, making it closer to a model trained only using
inlier samples. The details of the conducted experiments are presented in the
following section.

##  3 Experiments and results

In this section, we evaluate our proposed method on a simple real dataset, a
simulation-based dataset, and a medical image translation problem.

###  3.1 Star cluster CYB OB1

First, we start with a simple dataset on the star cluster CYB OB1 which was
analyzed in [11]. This dataset consists of 47 observations from which four
points with high leverage do not follow the trend of the rest of the data. It
has one explanatory variable which is the logarithm of the effective
temperature at the surface of stars. The independent variable is the logarithm
of its light intensity. The authors have shown the efficacy of least trimmed
quantile regression compared to quantile regression using linear programming
optimization to find the model’s parameters. However, our goal is to
investigate robustness in neural networks where the solution will be
calculated using stochastic gradient descent (SGD). We estimate 0.25, 0.5, and
0.75 quantiles with a neural network.

We implement the linear quantile regression problem with a one-layer neural
network with linear activation. Then we applied the three suggested robust
methods TQR, RCP, and $\beta$-QR. We used GD with the ADAM optimizer to train
the network. We chose the hyperparameters for each model (trimming percentage,
L and $\beta$) using a grid search. We used a batch size of 47 and performed
5000 iterations.

The results are shown in Fig. 1. For a quantitative comparison of the models
We calculated the Frobenius norm between each estimated quantile and the
solution, which was learned only using the inliers (Table 1 and Fig. 1). The
$\beta$-QR method shows the best performance among the methods. For optimizing
RCP cost, we used the Alternating Direction Method of Multipliers (ADMM) in
which we split the objective into
$\sum_{i}\rho_{\alpha}(y_{i}-f_{\theta}(x_{i})-\gamma_{i})$ and
$\lambda\sum_{i}|\gamma_{i}|$. We optimized the former using GD with the ADAM
optimizer, and for the latter, we used a proximal method for the $L_{1}$
objective:

$\displaystyle
proxy_{\lambda,l_{1}}(x_{i}):=\begin{cases}x_{i}-\lambda&\text{if
$x_{i}>\lambda$ }\\\ x_{i}+\lambda&\text{if $x_{i}<\lambda$ }\\\
0&\text{otherwise.}\end{cases}$ (5)

We iterated between optimization of the two components of the cost function
until convergence.

Fig. 1: Robust linear quantile regression using TQR, RCP, $\beta$ -QR for star
cluster CYB OB1 dataset.

###  3.2 Toy example for uncertainty estimation

Here we used the simple synthetic dataset introduced in [4] to which we added
1% outliers. Tagasovska and Lopez-Paz [4] applied simultaneous Quantile
Regression (SQR) to estimate aleatoric uncertainty and suggested estimating
all the quantile levels simultaneously. We modeled the data using a three-
layer neural network with ReLU activation function. We then applied the three
robust methods TQR, RCP, and $\beta$-QR. We used SGD with the ADAM optimizer
for training. We trained TQR and $\beta$-QR with a batch size of 128 and ran
each for 500 epochs. RCP was trained for ten epochs and 500 steps of iterative
optimization. We estimated the performance of the robust model for 0.25, 0.5
and 0.75 quantiles. Our results shown that $\beta$-QR estimates robust
quantiles and comparable results to TQR (Fig. 2).

Fig. 2: Robust non-linear quantile regression using TQR, RCP, $\beta$-QR using a simple neural network for a toy example.  Fig. 3: Estimating T2 MRI QL(0.05),QH(0.95),QM(0.5) for diffusion models from T1 MRI. Comparing the estimated quantiles using the non-robust and robust ($\beta$-QR) model with the outlier free model. Table 1: Comparison of performance of TQR, RCP and $\beta$-QR. Each entry shows the Frobenius norm of the difference between the estimated quantiles and their (outlier-free) ground truth for the star cluster CYB OB1 dataset.  Method | CYB-Q1 | CYB-Q2 | CYB-Q3
---|---|---|---
TQR | 1.04 | 1.12 | 1.12
RCP | 3.43 | 4.82 | 3.74
$\beta$-QR | 0.93 | 0.77 | 0.85

###  3.3 Quantile regression for uncertainty estimation in diffusion models

In this section, we present an experiment aimed at showcasing the
effectiveness of our proposed robust quantile regression approach in a medical
imaging task. Specifically, we focus on addressing the outlier problem in an
image translation task, where we employ a diffusion model to predict various
quantiles of T2-weighted brain MRI images based on input T1-weighted images.

Our training dataset consists of two distinct groups of subjects: (i) Lesion-
free subjects that come from the Cam-CAN dataset (in-liers) [21], representing
individuals without any brain lesions, and (ii) Lesion subjects that are
sourced from the BRATS dataset [22] who do have brain lesions, thereby
introducing outliers into the dataset. Training the diffusion model solely on
the Cam-CAN data and using the loss function in (1) yields a reliable model
that successfully captures the relationship between T1 and the quantiles of T2
images. However, introducing the “outlier” lesioned brain images from the
BRATS dataset into the training set and using the same loss function
significantly perturbs the training process, resulting in a notably less
reliable model and corrupted quantiles. To mitigate the adverse effects of the
outlier samples and restore model reliability, we integrate the proposed
robust loss function presented in (4) into the training process. This loss
function is designed to down-weight the influence of outliers during training,
effectively bringing the model’s performance closer to that of the model
trained solely on clean data from the Cam-CAN dataset. The robust loss
function was employed to train the model with the combined Cam-CAN and BRATS
data sets. The results of this experiment are illustrated in Fig. 3, providing
a qualitative comparison of the trained models. Table 2 shows our quantitative
results. These results unequivocally demonstrate that the inclusion of the
robust loss function during model training significantly enhances the model’s
robustness to outliers, resulting in a reliable model that closely
approximates the performance of the model trained exclusively on clean data.
We estimated 0.05,0.5 and 0.95 for this dataset. For comparison of the robust
and non-robust models, we calculated: (1) the MSE between the estimated
quantiles and the outlier-free model predicted quantiles; and (2) the MSE
between the predicted median and the ground-truth T2 image. We tuned the
$\beta$ parameter using a validation set.

##  4 Conclusion

In this paper we introduced a robust quantile regression approach designed to
enhance the reliability of deep learning models in the presence of outliers.
Our method leverages concepts from robust divergences to down-weight outlier
influence during training. We demonstrated the effectiveness of our approach
on a simple yet real dataset, showcasing its ability to improve quantile
regression accuracy compared to existing robust quantile regression methods.
Extending the application to medical imaging, and demonstrating its practical
utility, the proposed approach proved effective in mitigating outlier effects
on training a diffusion model to translate MRI brain images from a T1-weighted
to T2-weighted modality, bringing the performance closer to that of the model
trained solely on clean data. The presented findings highlight the practical
value of the proposed method, particularly in training scenarios compromised
by outliers.

Table 2: Comparing performance of $\beta$-QR with outlier free on baseline model. For the prediction error, MSE calculated between ground truth T2 and the median of each model  Method | Prediction error | Quantile error
---|---|---
Outlier free | 0.0086 | -
Baseline | 0.0132 | 0.0097
$\beta$-QR | 0.0074 | 0.0013
TQR | 0.0107 | 0.0015

## References

  * [1] Nicki Skafte, Martin Jørgensen, and Søren Hauberg,  “Reliable training and estimation of variance networks,”  in Advances in Neural Information Processing Systems, 2019, pp. 6323–6333.
  * [2] Yaniv Romano, Evan Patterson, and Emmanuel Candes,  “Conformalized quantile regression,”  in Advances in Neural Information Processing Systems, 2019, pp. 3543–3553.
  * [3] Jakob Gawlikowski, Cedrique Rovile Njieutcheu Tassi, Mohsin Ali, Jongseok Lee, Matthias Humt, Jianxiang Feng, Anna Kruspe, Rudolph Triebel, Peter Jung, Ribana Roscher, et al.,  “A survey of uncertainty in deep neural networks,”  Artificial Intelligence Review, pp. 1–77, 2023.
  * [4] Natasa Tagasovska and David Lopez-Paz,  “Single-model uncertainties for deep learning,”  in Advances in Neural Information Processing Systems, 2019, pp. 6417–6428.
  * [5] Haleh Akrami, Anand Joshi, Sergul Aydore, and Richard Leahy,  “Quantile regression for uncertainty estimation in vaes with applications to brain lesion detection,”  in International Conference on Information Processing in Medical Imaging. Springer, 2021, pp. 689–700.
  * [6] Haleh Akrami, Anand Joshi, Sergul Aydore, and Richard Leahy,  “Deep quantile regression for uncertainty estimation in unsupervised and supervised lesion detection,”  arXiv preprint arXiv:2109.09374, 2021.
  * [7] Anastasios N Angelopoulos, Amit Pal Kohli, Stephen Bates, Michael Jordan, Jitendra Malik, Thayer Alshaabi, Srigokul Upadhyayula, and Yaniv Romano,  “Image-to-image regression with distribution-free uncertainty quantification and applications in imaging,”  in International Conference on Machine Learning. PMLR, 2022, pp. 717–730.
  * [8] Keming Yu and Rana A Moyeed,  “Bayesian quantile regression,”  Statistics & Probability Letters, vol. 54, no. 4, pp. 437–447, 2001.
  * [9] Haleh Akrami, Anand A Joshi, Sergül Aydöre, and Richard M Leahy,  “Deep quantile regression for uncertainty estimation in unsupervised and supervised lesion detection,”  The journal of machine learning for biomedical imaging, vol. 1, 2022.
  * [10] Peter J Huber,  Robust statistics,  Springer, 2011.
  * [11] NM Neykov, Pavel Čížek, Peter Filzmoser, and PN Neytchev,  “The least trimmed quantile regression,”  Computational Statistics & Data Analysis, vol. 56, no. 6, pp. 1757–1770, 2012.
  * [12] Yiyuan She and Art B Owen,  “Outlier detection using nonconvex penalized regression,”  Journal of the American Statistical Association, vol. 106, no. 494, pp. 626–639, 2011.
  * [13] Ayanendranath Basu, Ian R Harris, Nils L Hjort, and MC Jones,  “Robust and efficient estimation by minimising a density power divergence,”  Biometrika, vol. 85, no. 3, pp. 549–559, 1998.
  * [14] Haleh Akrami, Anand A Joshi, Jian Li, Sergül Aydöre, and Richard M Leahy,  “A robust variational autoencoder using beta divergence,”  Knowledge-based systems, vol. 238, pp. 107886, 2022.
  * [15] Haleh Akrami, Sergul Aydore, Richrd M Leahy, and Anand A Joshi,  “Robust variational autoencoder for tabular data with beta divergence,”  arXiv preprint arXiv:2006.08204, 2020.
  * [16] Jonathan Ho, Ajay Jain, and Pieter Abbeel,  “Denoising diffusion probabilistic models,”  Advances in neural information processing systems, vol. 33, pp. 6840–6851, 2020.
  * [17] Chitwan Saharia, Jonathan Ho, William Chan, Tim Salimans, David J Fleet, and Mohammad Norouzi,  “Image super-resolution via iterative refinement,”  IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 4, pp. 4713–4726, 2022.
  * [18] Alexander Quinn Nichol and Prafulla Dhariwal,  “Improved denoising diffusion probabilistic models,”  in International Conference on Machine Learning. PMLR, 2021, pp. 8162–8171.
  * [19] Jascha Sohl-Dickstein, Eric Weiss, Niru Maheswaranathan, and Surya Ganguli,  “Deep unsupervised learning using nonequilibrium thermodynamics,”  in International conference on machine learning. PMLR, 2015, pp. 2256–2265.
  * [20] Chitwan Saharia, William Chan, Huiwen Chang, Chris Lee, Jonathan Ho, Tim Salimans, David Fleet, and Mohammad Norouzi,  “Palette: Image-to-image diffusion models,”  in ACM SIGGRAPH 2022 Conference Proceedings, 2022, pp. 1–10.
  * [21] Jason R Taylor, Nitin Williams, Rhodri Cusack, Tibor Auer, Meredith A Shafto, Marie Dixon, Lorraine K Tyler, Richard N Henson, et al.,  “The cambridge centre for ageing and neuroscience (cam-can) data repository: Structural and functional mri, meg, and cognitive data from a cross-sectional adult lifespan sample,”  neuroimage, vol. 144, pp. 262–269, 2017.
  * [22] Bjoern H Menze, Andras Jakab, Stefan Bauer, Jayashree Kalpathy-Cramer, Keyvan Farahani, Justin Kirby, Yuliya Burren, Nicole Porz, Johannes Slotboom, Roland Wiest, et al.,  “The multimodal brain tumor image segmentation benchmark (brats),”  IEEE transactions on medical imaging, vol. 34, no. 10, pp. 1993–2024, 2014.


# Flatness-based Vehicle Variable Horizon Optimal Control with Continuous-time
Safety Guarantees

Victor Freire, Xiangru Xu Victor Freire and Xiangru Xu are with the Department
of Mechanical Engineering, University of Wisconsin-Madison, Madison, WI, USA.
Email<EMAIL_ADDRESS>
###### Abstract

We present a convex optimization framework to find feasible solutions of an
optimal control problem for the bicycle kinematic model. The obtained state
and input trajectories are endowed with safety guarantees in the continuous-
time sense. Furthermore, the approach involves solving three second-order cone
programs, which are a special case of convex optimization. Thus, efficient
solvers exist and the approach is suitable for real-time (25-30 Hz)
applications. Finally, we present some simulation examples and comparisons
that demonstrate the usefulness of the presented approach.

##  I Introduction

[VF: Problem:] The vehicle motion planning problem is well-studied. However,
fast algorithms tend to lack formal safety guarantees while robust and safe
planning methods are usually slow, making them unfit for real-time
implementation. [VF: Motivation:] The search for a fast and safe planning
algorithm is key to achieving higher levels of driving autonomy
[shi_principles_2020].

[VF: Literature Review:] The literature addressing the motion planning problem
for vehicles is vast. A common approach is the spatio-temporal division of the
problem. That is, most algorithms focus separately on path finding
[karaman2011sampling, elbanhawi2015randomized, zhang2018hybrid] and speed
profile optimization [verscheure2009time, lipp2014minimum, zhu2015convex,
liu2017speed]. Path finding algorithms generate geometric paths that take the
vehicle from its current position to the goal position while avoiding
obstacles and respecting kinematic and nonholonomic constraints (kinodynamic
feasibility). Traditionally, these algorithms strive to find minimum length or
minimum curvature paths. Speed profile optimization algorithms usually assume
that an obstacle-free path is available and try to determine how fast the
vehicle should travel through this path. These algorithms usually focus on
achieving minimum time and maximum rider comfort. In
[elbanhawi2015randomized], the authors use RRT and B-spline curves to explore
the space and generate kinodynamically feasible paths. However, the increased
burden on each RRT sampling step to verify kinodynamic feasibility results in
increased computation times. In [zhang2018hybrid], a multi-layer planning
framework is proposed. The path finding layer uses sampling techniques to
modify a global path for obstacle avoidance. However, they rely on nonconvex
minimization of the path’s curvature to enforce kinodynamic feasibility. In
[verscheure2009time, lipp2014minimum], the speed profile optimization problem
over a given path is addressed. The authors in [verscheure2009time] showed
that a minimum-time objective function can be reformulated in terms of the
path parameter. A few years later, authors in [lipp2014minimum] showed that
the resulting problem is convex and generalized the results for certain
classes of systems.

Figure 1: Proposed framework block diagram. (PATH-SOCP) finds a safe path
$\boldsymbol{\mathbf{\theta}}(s)$, (TIME-SOCP) finds an appropriate trajectory
duration $t_{f}$ and (SPEED-SOCP) finds a safe speed profile $s(t)$. These
quantities parameterize the flat outputs which can be converted into a state-
space trajectory by the differential flatness property of the model
considered. The obtained trajectory $\boldsymbol{\mathbf{x}}(t)$ and
$\boldsymbol{\mathbf{u}}(t)$ is a guaranteed feasible point of (OPT) by
Theorem 1.

In [zhu2015convex], the authors use the spatio-temporal separation to
alternate optimizing over the path and over the speed profile. Both problems
are convex and by iterating this process, the algorithm produces high quality
trajectories. The need to iterate, however, introduces both ambiguity in the
stopping criteria and computational burden. In [liu2017speed], the authors
perform speed profile planning via temporal optimization to avoid dynamic
obstacles and maximize passenger comfort. They formulate a nonconvex problem
and use the slack convex feasible set algorithm to solve it. However, their
formulation is in the discrete-time domain. Thus, it fails to provide
guarantees in the continuous-time sense.

Another common approach for autonomous driving is using optimal control
methods, such as MPC, to track a given path (i.e., the road’s centerline). For
example, [kong2015kinematic] formulated a MPC problem based on the kinematic
bicycle model which has been widely used both in practice and as a starting
point for the design of robust and safe algorithms. However, the problem is
nonconvex and provides no safety guarantees in the continuous-time sense. In
[liniger2017racing], the authors demonstrated the high performance of
stochastic MPC in a miniature racing environment. However, their formulation’s
safety guarantees are probabilistic and cannot be certified. In
[werling2010optimal], the authors use the local vehicle frame to separately
plan optimal longitudinal and lateral movements. Their method, however,
requires short horizons and must adapt, by means of parameter tuning, to high
or low speed situations.

[VF: Proposed Method:] In this work, we formulate the motion planning problem
as an optimal control problem. Then, we use spatio-temporal division,
differential flatness and the convexity properties of B-spline functions to
formulate three second-order cone programs (SOCPs) yielding a path, a
trajectory duration, and a speed profile. We show that, by combining the SOCP
solutions, we can obtain a system trajectory which is a feasible but generally
suboptimal solution to the original optimal control problem. Notably, SOCPs
are a special type of convex optimization problems for which efficient solvers
exist. This makes the proposed framework suitable for real-time and embedded
applications. Furthermore, our framework provides trajectories with rigorous
continuous-time safety guarantees in the form of state and input constraints
including maximum steering angle, position constraints, maximum velocity and
maximum acceleration.

The remainder of the paper is organized as follows: Section II describes the
bicycle kinematic model and its differential flatness property, B-spline
curves, second-order cone constraints and introduces the optimal control
problem considered; Section III provides necessary and sufficient conditions
in flat space which guarantee safety in state space; Section IV presents
sufficient but convex (with respect to B-spline curves’ control points)
relaxations to the previous, generally nonconvex, necessary and suficient
conditions. This section also presents the second-order cone programs whose
solutions are used to obtain feasible state-space trajectories in the original
optimal control problem; Section V demonstrates the safety constraints,
provides a performance comparison with state-of-the-art optimal control
solvers and showcases the proposed framework in a scenario navigating Mcity;
Finally, Section VI concludes the paper.

##  II Preliminaries & Problem Statement

###  II-A Bicycle Kinematic Model

The bicycle kinematic model is a commonly used, simple model which captures
the nonholonomic constraint present in most wheeled vehicles
[polack2017kinematic]. This is usually enough to generate suitable
trajectories for a model-based controller which may consider higher-fidelity
vehicle dynamics [polack2018guaranteeing].

We consider the bicycle kinematic model expressed as

$\dot{\boldsymbol{\mathbf{x}}}=f(\boldsymbol{\mathbf{x}})+g(\boldsymbol{\mathbf{x}})\boldsymbol{\mathbf{u}}=\begin{bmatrix}v\cos\psi\\\
v\sin\psi\\\ 0\\\ 0\end{bmatrix}+\begin{bmatrix}0&0\\\ 0&0\\\ 1&0\\\
0&1\end{bmatrix}\boldsymbol{\mathbf{u}},$ (1)

where

$\displaystyle\boldsymbol{\mathbf{x}}$
$\displaystyle=\begin{bmatrix}x&y&v&\psi\end{bmatrix}^{T},$ (2a)
$\displaystyle\boldsymbol{\mathbf{u}}$
$\displaystyle=\begin{bmatrix}\dot{v}&\dot{\psi}\end{bmatrix}^{T},$ (2b)

are the state and input vectors, respectively, $(x,y)$ is the position of the
rear wheel, $v$ is the magnitude of the velocity vector and $\psi$ is the
heading with respect to the inertial frame’s $x$-axis (see Figure 2).

Figure 2: Bicycle kinematic model and world (inertial) coordinate frame.[XX:
Is the picture drawn by yourself or borrowed from somewhere?][VF: I made it in
PowerPoint]

The following quantities are also relevant:

$\gamma=\arctan\bigg{(}\frac{L\dot{\psi}}{v}\bigg{)},\quad
v_{F}=\frac{v}{\cos\gamma},$ (3)

where $\gamma$ is the front wheel steering angle, $v_{F}$ is the speed of the
front wheel and $L>0$ is the wheelbase length (see Figure 2). The considered
kinematic bicycle model is a special case of the classical _n-cart_ system
[rouchon1993flatness] and is known to be _differentially flat_
[fliess1995flatness]. By choosing flat outputs as:

$\boldsymbol{\mathbf{y}}=\begin{bmatrix}x&y\end{bmatrix}^{T},$

the state and input can be expressed as functions of $\boldsymbol{\mathbf{y}}$
and a finite number of its derivatives:

$\displaystyle\boldsymbol{\mathbf{x}}$
$\displaystyle=\Phi(\boldsymbol{\mathbf{y}},\dot{\boldsymbol{\mathbf{y}}}),$
(4a) $\displaystyle\boldsymbol{\mathbf{u}}$
$\displaystyle=\Psi(\boldsymbol{\mathbf{y}},\dot{\boldsymbol{\mathbf{y}}},\ddot{\boldsymbol{\mathbf{y}}})$
(4b)

where the expressions of $\Phi$ and $\Psi$ are given as in
[rouchon1993flatness]:

$\displaystyle v$ $\displaystyle=\sqrt{\dot{x}^{2}+\dot{y}^{2}},$
$\displaystyle\psi$
$\displaystyle=\arctan\bigg{(}\frac{\dot{y}}{\dot{x}}\bigg{)},$
$\displaystyle\dot{v}$
$\displaystyle=\frac{\dot{x}\ddot{x}+\dot{y}\ddot{y}}{\sqrt{\dot{x}^{2}+\dot{y}^{2}}},$
$\displaystyle\dot{\psi}$
$\displaystyle=\frac{\ddot{y}\dot{x}-\ddot{x}\dot{y}}{\dot{x}^{2}+\dot{y}^{2}}.$

Generating a trajectory for differentially flat systems reduces to finding a
sufficiently smooth flat output trajectory [van1998real]. In the case of
system (1), the flat trajectory $\boldsymbol{\mathbf{y}}(t)$ needs to be at
least twice-differentiable.

###  II-B B-Spline Curves

B-splines are commonly used basis functions endowed with powerful smoothness
and convexity properties. We will make use of their smoothness to satisfy the
differentiability constraint of the flat output trajectory
$\boldsymbol{\mathbf{y}}(t)$. We will also use their convexity to satisfy
safety constraints in continuous time. B-splines have been used widely in the
literature for the purpose of trajectory generation [elbanhawi2015randomized,
stoican2017constrained].

A $d$-th degree _B-spline basis_ with $d\in\mathbb{Z}_{>0}$ is defined over a
given _knot vector_
$\boldsymbol{\mathbf{\tau}}=(\tau_{0},\ldots,\tau_{\nu})^{T}$ satisfying
$\tau_{i}\leq\tau_{i+1}$ for $i=0,\ldots,v-1$ and it can be computed
recursively as [de1978practical]:

$\displaystyle\lambda_{i,0}(t)$ $\displaystyle=\begin{cases}1,&\tau_{i}\leq
t<\tau_{i+1},\\\ 0,&\text{otherwise},\\\ \end{cases}$
$\displaystyle\lambda_{i,d}(t)$
$\displaystyle=\frac{t-\tau_{i}}{\tau_{i+d}-\tau_{i}}\lambda_{i,d-1}(t)+\frac{\tau_{i+d+1}-t}{\tau_{i+d+1}-\tau_{i+1}}\lambda_{i+1,d-1}(t).$

Additionally, we consider the _clamped, uniform B-spline basis_ , which is
defined over knot vectors satisfying:

(clamped)
$\displaystyle\tau_{0}=\ldots=\tau_{d},\quad\tau_{\nu-d}=\ldots=\tau_{\nu},$
(5a) (uniform) $\displaystyle\tau_{d+1}-\tau_{d}=\ldots=\tau_{N+1}-\tau_{N},$
(5b)

where $N=\nu-d-1$. A $d$-th degree _B-spline curve_
$\boldsymbol{\mathbf{s}}(t)$ is a $m$-dimensional parametric curve built by
linearly combining _control points_
$\boldsymbol{\mathbf{p}}_{i}\in\mathbb{R}^{m}(i=0,\ldots,N)$ and B-spline
bases of the same degree. Noting
$\boldsymbol{\mathbf{s}}^{(0)}(t)=\boldsymbol{\mathbf{s}}(t)$, we generate a
B-spline curve and its $r$-th order derivative by:

$\boldsymbol{\mathbf{s}}^{(r)}(t)=\sum_{i=0}^{N}\boldsymbol{\mathbf{p}}_{i}\boldsymbol{\mathbf{b}}_{r,i+1}^{T}\boldsymbol{\mathbf{\Lambda}}_{d-r}(t)=PB_{r}\boldsymbol{\mathbf{\Lambda}}_{d-r}(t),$
(6)

where the control points are grouped into a matrix

$P=\begin{bmatrix}\boldsymbol{\mathbf{p}}_{0}&\dots&\boldsymbol{\mathbf{p}}_{N}\end{bmatrix}\in\mathbb{R}^{m\times(N+1)},$
(7)

the basis functions are grouped into a vector

$\boldsymbol{\mathbf{\Lambda}}_{d-r}(t)=\left[\lambda_{0,d-r}(t),\dots,\lambda_{N+r,d-r}(t)\right]^{T}\in\mathbb{R}^{N+r+1},$
(8)

and $\boldsymbol{\mathbf{b}}_{r,j}^{T}$ is the $j$-th row of a time-invariant
matrix $B_{r}\in\mathbb{R}^{(N+1)\times(N+r+1)}$ [suryawan2012constrained]
constructed as:

$B_{r}=M_{d,d-r}C_{r},$ (9)

where matrices $M_{d,d-r}\in\mathbb{R}^{(N+1)\times(N-r+1)}$ and
$C_{r}\in\mathbb{R}^{(N-r+1)\times(N+r+1)}$ are defined in
[freire2021flatness].

######  Definition 1.

_[ freire2021flatness]_ The columns of $P^{(r)}\triangleq PB_{r}$ are called
the _$r$ -th order virtual control points_ (VCPs) of
$\boldsymbol{\mathbf{s}}^{(r)}(t)$ and denoted as
$\boldsymbol{\mathbf{p}}_{i}^{(r)}$ where $i=0,1,\ldots,N+r$, i.e.,

$P^{(r)}=PB_{r}=\begin{bmatrix}\boldsymbol{\mathbf{p}}_{0}^{(r)}&\dots&\boldsymbol{\mathbf{p}}_{N+r}^{(r)}\end{bmatrix}.$
(10)

######  Proposition 1.

_[ freire2021flatness]_ Given a convex set $\mathcal{S}$ and the $r$-th
derivative of a clamped B-spline curve $\boldsymbol{\mathbf{s}}^{(r)}(t)$
defined as in (6), if

$\boldsymbol{\mathbf{p}}_{j}^{(r)}\in\mathcal{S},\quad j=r,\ldots,N$ (11)

holds, then $\boldsymbol{\mathbf{s}}^{(r)}(t)\in\mathcal{S},\
t\in[\tau_{0},\tau_{\nu})$. If

$\boldsymbol{\mathbf{p}}^{(r)}_{j}\in S,\quad j=i-d+r,\dots,i,\quad
i\in\\{d,\ldots,N\\}$ (12)

holds, then $\boldsymbol{\mathbf{s}}^{(r)}(t)\in\mathcal{S},\
t\in[\tau_{i},\tau_{i+1})$.

###  II-C Second-order Cone Constraints

_Second-order cone programs_ (SOCP) are well-studied, special cases of convex
optimization problems [alizadeh2003second]. They have the following form
[lobo1998applications, boyd2004convex]:

$\displaystyle\min\quad$
$\displaystyle\boldsymbol{\mathbf{f}}^{T}\boldsymbol{\mathbf{x}}$ (13) s.t.
$\displaystyle\|A_{i}\boldsymbol{\mathbf{x}}+\boldsymbol{\mathbf{b}}_{i}\|_{2}\leq\boldsymbol{\mathbf{c}}_{i}^{T}\boldsymbol{\mathbf{x}}+d_{i},\quad
i=1,\ldots,m,$ $\displaystyle
F\boldsymbol{\mathbf{x}}=\boldsymbol{\mathbf{g}},$

where $\boldsymbol{\mathbf{x}}\in\mathbb{R}^{n}$ is the decision variable. The
parameters are: $\boldsymbol{\mathbf{f}}\in\mathbb{R}^{n}$,
$A_{i}\in\mathbb{R}^{(n_{i}-1)\times n}$,
$\boldsymbol{\mathbf{b}}_{i}\in\mathbb{R}^{n_{i}-1}$,
$\boldsymbol{\mathbf{c}}_{i}\in\mathbb{R}^{n}$, $d_{i}\in\mathbb{R}$,
$F\in\mathbb{R}^{m\times n}$ and $\boldsymbol{\mathbf{g}}\in\mathbb{R}^{m}$.
Particularly, $n$-dimensional _second-order cone_ (SOC) constraints have the
form:

$\|A\boldsymbol{\mathbf{x}}+\boldsymbol{\mathbf{b}}\|_{2}\leq\boldsymbol{\mathbf{c}}^{T}\boldsymbol{\mathbf{x}}+d.$
(14)

SOCP can be seen as a generalization of certain, more specialized types of
convex optimization problems. Some examples include linear programs (LP),
quadratic programs (QP) and convex, quadratically constrained quadratic
programs (QCQP) [alizadeh2003second, lobo1998applications, boyd2004convex].
Most notably with regard to this paper’s results, it has been shown that SOCPs
can be solved in polynomial time by interior-point methods
[potra2000interior]. In addition, efficient and specialized solvers such as
MOSEK have been developed [mosek].

###  II-D Problem Statement

We formulate the motion planning problem of a car as the following constrained
optimal control problem with variable horizon:

$\displaystyle\min\limits_{\boldsymbol{\mathbf{x}}(\cdot),\boldsymbol{\mathbf{u}}(\cdot),t_{f}}\quad$
$\displaystyle\nu
t_{f}+\int_{0}^{t_{f}}L(\boldsymbol{\mathbf{x}}(t),\boldsymbol{\mathbf{u}}(t))\;\mathrm{d}t$
(OPT) s. t.
$\displaystyle\dot{\boldsymbol{\mathbf{x}}}(t)=f(\boldsymbol{\mathbf{x}}(t))+g(\boldsymbol{\mathbf{x}}(t))\boldsymbol{\mathbf{u}}(t),$
(15a)
$\displaystyle\boldsymbol{\mathbf{x}}(0)=\boldsymbol{\mathbf{x}}_{0},\quad\boldsymbol{\mathbf{x}}(t_{f})=\boldsymbol{\mathbf{x}}_{f},$
(15b) $\displaystyle 0\leq v(t)\leq\overline{v},\quad\forall t\in[0,t_{f}],$
(15c) $\displaystyle|\dot{v}(t)|\leq\overline{a},\quad\forall t\in[0,t_{f}],$
(15d) $\displaystyle|\gamma(t)|\leq\overline{\gamma},\quad\forall
t\in[0,t_{f}],$ (15e)
$\displaystyle\boldsymbol{\mathbf{r}}(t)\in\mathcal{D},\quad\forall
t\in[0,t_{f}],$ (15f)

where (15a) is the system model as shown in (1); (15b) is the initial and
final state constraints; (15c) is the speed limit constraint with
$\overline{v}>0$; (15d) is the maximum acceleration and braking constraint
with $\overline{a}>0$; (15e) is the steering angle bound constraint with
$0<\overline{\gamma}<\pi/2$; and (15f) is the road boundaries (obstacle
avoidance) constraint with $\boldsymbol{\mathbf{r}}\triangleq(x,y)^{T}$ the
position of the car and $\mathcal{D}$ the obstacle-free region; the Lagrange
cost functional $L:\mathbb{R}^{4}\times\mathbb{R}^{2}\rightarrow\mathbb{R}$ is
chosen to promote smoothness properties for the trajectory; the parameter
$\nu>0$ encodes the tradeoff between time-optimality and smoothness to be
specified by the user.

The problem that we will investigate in this paper is that of finding
solutions to (OPT) in real time. While we won’t compromise on safety
(feasibility), we will trade optimality for an increase in computational
efficiency. Our approach leverages the differential flatness property of the
bicycle model and parameterizes flat outputs using a pair of convoluted
B-spline curves whose convexity properties will allow us to verify such
constraints formally in continuous-time. We consider a separation between
space ($\mathbb{R}^{2}$) and time to first find a _path_ with desirable
properties; then, we use these properties to find a _speed profile_ for
navigating it. Convoluting the _path_ with its _speed profile_ results in the
flat output trajectory required to recover the state-space trajectory. This
approach of considering function convolutions as the flat outputs was first
presented in [martin2006flat] to avoid singularities (such as division by
zero) that sometimes appear in flat maps such as (4).

##  III Safety Constraints in Flat Output Space

In this section, we provide necessary and sufficient, generally non-convex
conditions on the flat output trajectory $\boldsymbol{\mathbf{y}}(t)$ that
guarantee safety in the state-space.

We begin by considering a _path_
$\boldsymbol{\mathbf{\theta}}(s)\triangleq\big{(}x(s),y(s)\big{)}^{T}\in
C^{2}:[0,1]\rightarrow\mathbb{R}^{2}$ and _speed profile_ $s\in
C^{2}:[0,t_{f}]\rightarrow[0,1]$, where $C^{n}$ denotes the set of smooth
functions whose derivatives, up to $n$-th order, exist and are continuous.
Together, the path and speed profiles completely define the flat outputs
[pedrosa2003point]

$\boldsymbol{\mathbf{y}}(t)=\boldsymbol{\mathbf{\theta}}\big{(}s(t)\big{)},$
(16)

whose derivatives are computed as

$\displaystyle\dot{\boldsymbol{\mathbf{y}}}(t)$
$\displaystyle=\dot{s}(t)\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)},$
(17a) $\displaystyle\ddot{\boldsymbol{\mathbf{y}}}(t)$
$\displaystyle=\ddot{s}(t)\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}+\dot{s}^{2}(t)\boldsymbol{\mathbf{\theta}}^{\prime\prime}\big{(}s(t)\big{)},$
(17b)

where $\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}$ denotes
differentiation of $\boldsymbol{\mathbf{\theta}}$ with respect to $s$ and
taking values at $s(t)$, and similarly for
$\boldsymbol{\mathbf{\theta}}^{\prime\prime}\big{(}s(t)\big{)}$. The
parameterization shown in (16)-(17) provides a number of benefits: First, the
map (4) has singularities if $\|\dot{\boldsymbol{\mathbf{y}}}(t)\|_{2}=0$ for
some $t$ (i.e., $\dot{x}(t)=\dot{y}(t)=0$). We will see that this
parameterization allows us to avoid the singularity even in zero-speed
situations. Second, the position $\boldsymbol{\mathbf{r}}$ and the flat
mapping $\psi(t)$ in (4) depend only on the path:

$\displaystyle\psi(t)=\arctan\bigg{(}\frac{y^{\prime}(s)}{x^{\prime}(s)}\bigg{)},$

which will allow us to consider obstacle avoidance and steering angle
constraints solely when finding the path, independently of the speed profile
chosen later. In the following, we will overload the notation of the flat map
(4) as follows for convenience:

$\displaystyle\Phi(t)$
$\displaystyle\triangleq\Phi\big{(}\boldsymbol{\mathbf{y}}(t),\dot{\boldsymbol{\mathbf{y}}}(t)\big{)}=\boldsymbol{\mathbf{x}}(t),$
(18a) $\displaystyle\Psi(t)$
$\displaystyle\triangleq\Psi\big{(}\boldsymbol{\mathbf{y}}(t),\dot{\boldsymbol{\mathbf{y}}}(t),\ddot{\boldsymbol{\mathbf{y}}}(t)\big{)}=\boldsymbol{\mathbf{u}}(t),$
(18b)

where $\boldsymbol{\mathbf{y}}(t)$ and its derivatives are parameterized as in
(16). [XX: Should we include parameter $s$ in the equations above? ] [VF: Up
to you, the overloading is just to save space. But, we could instead define
$\Phi(\boldsymbol{\mathbf{\theta}},s)$, for example, which would make (FLAT-
OPT) more clear.]

###  III-A Path Safety

Define the _steering angle safety set_ :

$\mathcal{S}_{\gamma}\triangleq\\{(\boldsymbol{\mathbf{x}},\boldsymbol{\mathbf{u}})\in\mathbb{R}^{4}\times\mathbb{R}^{2}:|\gamma|\leq\overline{\gamma}<\pi/2\\},$
(19)

where $\gamma$ is defined in (3), and the _drivable safety set_ :

$\mathcal{S}_{D}\triangleq\\{\boldsymbol{\mathbf{x}}\in\mathbb{R}^{4}:\boldsymbol{\mathbf{r}}=(x,y)^{T}\in\mathcal{D}\\},$
(20)

where $\mathcal{D}\subseteq\mathbb{R}^{2}$ is the obstacle-free space.

######  Lemma 1.

The state-space trajectory
$\big{(}\Phi(t),\Psi(t)\big{)}\in\mathcal{S}_{\gamma}$ for all $t\in[0,t_{f}]$
if and only if

$\frac{\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\times\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}}{\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}^{3}}\leq\frac{\tan\overline{\gamma}}{L},\quad\forall
s\in[0,1].$ (21)

###### Proof.

It can be shown from (3) and the considered parameterization that:

$\gamma(t)=\arctan\bigg{(}\frac{L\big{(}y^{\prime\prime}(s)x^{\prime}(s)-x^{\prime\prime}(s)y^{\prime}(s)\big{)}}{\big{(}x^{\prime}(s)^{2}+y^{\prime}(s)^{2}\big{)}^{3/2}}\bigg{)}.$
(22)

Furthermore, over the range $\gamma\in(-\pi/2,\pi/2)$,
$\tan|\gamma|=|\tan\gamma|$. Therefore:

$\frac{\tan|\gamma|}{L}=\frac{|y^{\prime\prime}(s)x^{\prime}(s)-x^{\prime\prime}(s)y^{\prime}(s)|}{\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}^{3}}=\frac{\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\times\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}}{\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}^{3}}.$

Observing that

$\tan(|\gamma|)\leq\tan(\overline{\gamma}),\gamma\in(-\pi/2,\pi/2)\iff|\gamma|\leq\overline{\gamma}<\pi/2,$

the conclusion follows immediately. ∎

######  Lemma 2.

The state-space trajectory $\Phi(t)\in\mathcal{S}_{D}$ for all $t\in[0,t_{f}]$
if and only if

$\boldsymbol{\mathbf{\theta}}(s)\in\mathcal{D},\quad\forall s\in[0,1].$ (23)

###### Proof.

Recall that the speed profile $s\in C^{2}:[0,t_{f}]\rightarrow[0,1]$. Thus,
the condition
$\boldsymbol{\mathbf{y}}(t)=\boldsymbol{\mathbf{\theta}}(s(t))\in\mathcal{D}$
must hold for all $t\in[0,t_{f}]$. The conclusion follows by the definition of
$\mathcal{S}_{D}$. ∎

###  III-B Speed Profile Safety

Define the _forward speed safety set_ :

$\mathcal{S}_{v}\triangleq\\{\boldsymbol{\mathbf{x}}\in\mathbb{R}^{4}:0\leq
v\leq\overline{v}\\}.$ (24)

where $\overline{v}$ is the maximum forward speed, and the _linear
acceleration safety set_ :

$\mathcal{S}_{\dot{v}}\triangleq\\{\boldsymbol{\mathbf{u}}\in\mathbb{R}^{2}:|\dot{v}|\leq\overline{a}\\},$
(25)

where $\overline{a}$ is the maximum acceleration.

######  Lemma 3.

Let $\boldsymbol{\mathbf{\theta}}$ be a path. The state-space trajectory
$\Phi(t)\in\mathcal{S}_{v}$ for all $t\in[0,t_{f}]$ if and only if

$\dot{s}(t)\geq
0,\quad\dot{s}(t)\|\boldsymbol{\mathbf{\theta}}(s(t))\|_{2}\leq\overline{v},\quad\forall
t\in[0,t_{f}].$ (26)

###### Proof.

The conclusion follows directly from the flat map $\eqref{flat_map}$
describing the state $v$ and the considered parameterization of
$\dot{\boldsymbol{\mathbf{y}}}(t)$ given in (17a). ∎

######  Lemma 4.

Let $\boldsymbol{\mathbf{\theta}}$ be a path. The input trajectory
$\Psi(t)\in\mathcal{S}_{\dot{v}}$ for all $t\in[0,t_{f}]$ if and only if

$|a_{t}(t)+a_{n}(t)|\leq\overline{a},\quad\forall t\in[0,t_{f}],$ (27)

where

$\displaystyle a_{t}(t)$
$\displaystyle\triangleq\ddot{s}(t)\|\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\|_{2},$
$\displaystyle a_{n}(t)$
$\displaystyle\triangleq\frac{\dot{s}^{2}(t)\Big{(}\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\cdot\boldsymbol{\mathbf{\theta}}^{\prime\prime}\big{(}s(t)\big{)}\Big{)}}{\|\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\|_{2}}.$

###### Proof.

Differentiating
$v(t)=\dot{s}(t)\|\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\|_{2}$
with respect to time we have $\dot{v}(t)=a_{t}(t)+a_{n}(t)$. The conclusion
follows immediately by the definition of $\mathcal{S}_{\dot{v}}$. ∎

###  III-C Flattened Optimal Control Problem

Recall that $\Phi(t)$ and $\Psi(t)$, defined as in (18), each depend on the
_path_ $\boldsymbol{\mathbf{\theta}}$ and _speed profile_ $s$ [XX: Need to
include parameter $s$ in the expressions $\Phi(t)$ and $\Psi(t)$ defined as in
(18)?] [VF: See my previous comment by (18)]. Consider now the following
functional optimization problem:

$\displaystyle\min\limits_{\boldsymbol{\mathbf{\theta}}(\cdot),s(\cdot),t_{f}}\quad$
$\displaystyle\nu
t_{f}+\int_{0}^{t_{f}}L\big{(}\Phi(t),\Psi(t)\big{)}\;\mathrm{d}t,$ (FLAT-OPT)
s. t.
$\displaystyle\Phi(0)=\boldsymbol{\mathbf{x}}_{0},\quad\Phi(t_{f})=\boldsymbol{\mathbf{x}}_{f},$
$\displaystyle\text{\eqref{ncvx_gamma_cst}, \eqref{ncvx_obs_cst},
\eqref{ncvx_v_cst} and \eqref{ncvx_v_dot_cst} hold},$

where $\boldsymbol{\mathbf{\theta}}:[0,1]\rightarrow\mathbb{R}^{2}$ and
$s:[0,t_{f}]\rightarrow[0,1]$.

######  Corollary 1.

Let the duration $t_{f}$, _path_ $\boldsymbol{\mathbf{\theta}}$ and _speed
profile_ $s$ be a solution of (FLAT-OPT). Then, the corresponding state-space
trajectory given by $\boldsymbol{\mathbf{x}}(t)=\Phi(t)$ and
$\boldsymbol{\mathbf{u}}(t)=\Psi(t)$ is a solution of (OPT).

###### Proof.

The constraints on the initial and final states hold:

$\Phi(0)=\boldsymbol{\mathbf{x}}(0)=\boldsymbol{\mathbf{x}}_{0},\quad\Phi(t_{f})=\boldsymbol{\mathbf{x}}(t_{f})=\boldsymbol{\mathbf{x}}_{f}.$

By the definition of each safety set
$\mathcal{S}_{\gamma},\mathcal{S}_{D},\mathcal{S}_{v}$ and
$\mathcal{S}_{\dot{v}}$ and by Lemmas 1, 2, 3 and 4, it follows that the
state-space trajectory $\boldsymbol{\mathbf{x}}(t)$ and
$\boldsymbol{\mathbf{u}}(t)$ satisfies all safety constraints in (OPT). The
differential constraint in (OPT) is automatically satisfied by virtue of the
differential flatness property [van1998real] and smoothness ($C^{2}$) of the
_path_ $\boldsymbol{\mathbf{\theta}}$ and _speed profile_ $s$. Finally, notice
that the objective functionals are identical in both problems because
$\Phi(t)=\boldsymbol{\mathbf{x}}(t)$ and $\Psi(t)=\boldsymbol{\mathbf{u}}(t)$.
∎

Corollary 1 is a particular case of the observation in
[ross2002pseudospectral] that for differentially flat systems, optimal control
problems can be reformulated as functional optimization problems without
differential constraints by virtue of the flatness property. Intuitively, the
state differential constraint is translated into a smoothness constraint in
the flat output.

While we have eliminated the differential constraint (1), (FLAT-OPT) is still
intractable because the problem is nonconvex and we are minimizing over
functions instead of vectors. In the next section, we will let the path
$\boldsymbol{\mathbf{\theta}}$ and the speed profile $s$ be B-spline curves
and optimize over their control points. Furthermore, we will use their
convexity properties to formulate sufficient conditions which are convex with
respect to their control points.

##  IV Convexification of Problem (FLAT-OPT)

In this section, we describe a systematic convexification approach for Problem
(FLAT-OPT). We split the problem into three sequential steps, each involving a
SOCP: the first SOCP finds a safe path $\boldsymbol{\mathbf{\theta}}(s)$, the
second SOCP finds a duration $t_{f}$ for the trajectory, and the third SOCP
computes a safe velocity profile $s(t)$ (see Figure 1). The solution of these
three convex programs together provides a feasible but possibly sub-optimal
solution to Problem (FLAT-OPT) with rigorous continuous-time guarantees.

######  Definition 2 (B-spline path).

A _B-spline path_ is a $C^{2}$, $2$-dimensional, $d_{\theta}$-degree B-spline
curve defined as in (6) over a clamped, uniform knot vector
$\boldsymbol{\mathbf{\zeta}}$ segmenting the interval $[0,1]$ and control
points
$\boldsymbol{\mathbf{\Theta}}_{j}\in\mathbb{R}^{2},\;j=0,\ldots,N_{\theta}$.

######  Definition 3 (B-spline speed profile).

A _B-spline speed profile_ is a $C^{2}$, $1$-dimensional, $d_{s}$-degree
B-spline curve defined as in (6) over a clamped, uniform knot vector
$\boldsymbol{\mathbf{\tau}}$ segmenting the interval $[0,t_{f}]$ and control
points $p_{j}\in\mathbb{R},\;j=0,\ldots,N_{s}$.

TABLE I: Problem data for the proposed framework. $\boldsymbol{\mathbf{x}}_{0}$, $\boldsymbol{\mathbf{x}}_{f}$ | Initial and final state vectors, respectively.
---|---
$\overline{\gamma}$ | Maximum steering angle [rad].
$\mathcal{D}$ | Obstacle-free space ($\mathcal{D}\subseteq\mathbb{R}^{2}$).
$\overline{v}$ | Maximum speed [m/s].
$\overline{a}$ | Maximum acceleration and braking [m/$s^{2}$].
$\nu$ | Duration penalty factor.
$L$ | Wheelbase length as in Figure 2.

###  IV-A B-spline Path Optimization

The following Propositions can be thought of as sufficient, convex conditions
relaxing Lemmas 1 and 2.

######  Proposition 2.

Let $\boldsymbol{\mathbf{\theta}}(s)$ be a B-spline path and $b_{r,i,j}$ be
the $(i,j)$-th entry of matrix $B_{r}$ as shown in (9). If there exists
positive constant $\alpha>0$, column unit vector
$\hat{\boldsymbol{\mathbf{r}}}\in\mathbb{R}^{2}$, and variables
$\underline{v}_{\theta},\overline{a}_{\theta},\beta\in\mathbb{R}$ such that
the B-spline path $\boldsymbol{\mathbf{\theta}}(s)$ satisfies the following
conditions:

$\displaystyle\sum_{i=0}^{N_{\theta}}\hat{\boldsymbol{\mathbf{r}}}^{T}\boldsymbol{\mathbf{\Theta}}_{i}b_{1,i+1,j+1}$
$\displaystyle\geq\underline{v}_{\theta},\quad j=1,\ldots,N_{\theta},$ (28a)
$\displaystyle\bigg{\|}\sum_{i=0}^{N_{\theta}}\boldsymbol{\mathbf{\Theta}}_{i}b_{2,i+1,j+1}\bigg{\|}_{2}$
$\displaystyle\leq\overline{a}_{\theta},\quad j=2,\ldots,N_{\theta},$ (28b)
$\displaystyle\bigg{\|}\begin{bmatrix}2\alpha\\\
\frac{4\tan\overline{\gamma}}{L}\beta-1\end{bmatrix}\bigg{\|}_{2}$
$\displaystyle\leq\frac{4\tan\overline{\gamma}}{L}\beta+1,$ (28c)
$\displaystyle\overline{a}_{\theta}$
$\displaystyle\leq\alpha\underline{v}_{\theta}-\beta,$ (28d)
$\displaystyle\beta,\underline{v}_{\theta}$ $\displaystyle\geq 0,$ (28e)

then the state-space trajectory
$\big{(}\boldsymbol{\mathbf{x}}(t),\boldsymbol{\mathbf{u}}(t)\big{)}\in\mathcal{S}_{\gamma},$
$\forall t\in[0,t_{f}]$ where $\mathcal{S}_{\gamma}$ is the _steering angle
safety set_ defined in (19).

###### Proof.

Conditions (28a)-(28b) imply, by Proposition 1, that
$\underline{v}_{\theta}\leq\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}$
and
$\|\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}\leq\overline{a}_{\theta}$
for all $s\in[0,1)$. Continue by expanding (28c) to observe that

$0\geq\alpha^{2}-\frac{4\tan\overline{\gamma}}{L}\beta\triangleq\Delta_{v}.$

Notice that $\Delta_{v}$ is the discriminant of the quadratic polynomial in
$\underline{v}_{\theta}$ given by
$p(\underline{v}_{\theta})\triangleq\frac{\tan\overline{\gamma}}{L}\underline{v}_{\theta}^{2}-\alpha\underline{v}_{\theta}+\beta$.
It follows from $\Delta_{v}\leq 0$ that the roots of
$p(\underline{v}_{\theta})$ are either repeated and real, or complex
conjugates. Therefore, the polynomial $p(\underline{v}_{\theta})$ does not
change sign. Since $p(0)\geq 0$ (because $\beta\geq 0$), we must have that
$p(\underline{v}_{\theta})\geq 0$. In particular, we can now observe that

$\overline{a}_{\theta}\leq\alpha\underline{v}_{\theta}-\beta\leq\frac{\tan\overline{\gamma}}{L}\underline{v}_{\theta}^{2}\implies\|\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}\leq\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}^{2}\frac{\tan\overline{\gamma}}{L}$

holds. Now multiply by $\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}\geq
0$ in both sides of the implied inequality to establish:

$\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}^{3}\frac{\tan\overline{\gamma}}{L}\geq\|\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\|_{2}\geq\|\boldsymbol{\mathbf{\theta}}^{\prime}(s)\times\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|_{2}.$

The conclusion now follows directly from Lemma 1. ∎

######  Proposition 3.

Let $\mathcal{D}\subseteq\mathbb{R}^{2}$ be a given SOC. If the control points
$\boldsymbol{\mathbf{\Theta}}_{j}$ of the B-spline path
$\boldsymbol{\mathbf{\theta}}(s)$ satisfy the following conditions:

$\displaystyle\boldsymbol{\mathbf{\Theta}}_{j}\in\mathcal{D},\quad
j=0,\ldots,N_{\theta},$ (29)

then the state-space trajectory $\boldsymbol{\mathbf{x}}(t)\in\mathcal{D}$ for
all $t\in[0,t_{f}]$.

###### Proof.

Condition (29) implies, by Proposition 1, that
$\boldsymbol{\mathbf{\theta}}(s)\in\mathcal{D}$ for all $s\in[0,1)$. The
conclusion now follows directly by Lemma 2. ∎

######  Remark 1.

While the obstacle-free space $\mathcal{D}$ is generally nonconvex, the
convexity assumption is easily relaxed by considering the concept of “safe
corridor” (union of convex sets) and enforcing the conditions of Proposition 3
segment-wise instead of globally. The reader is referred to our previous work
[freire2021flatness] and to [gao2018online, sun2020fast] for more information.

For fixed values of $\alpha$ and $\hat{\boldsymbol{\mathbf{r}}}$, the
conditions of Proposition 2 are convex. We formulate the following SOCP to
obtain the path:

min.
$\displaystyle\int_{0}^{1}\|\boldsymbol{\mathbf{\theta}}^{\prime\prime\prime}(s)\|_{2}^{2}\;\mathrm{d}s+\overline{v}_{\theta}-\underline{v}_{\theta}+\overline{a}_{\theta}$
(PATH-SOCP) s. t.
$\displaystyle\boldsymbol{\mathbf{\Theta}}_{0}=\boldsymbol{\mathbf{r}}_{0},\quad\boldsymbol{\mathbf{\Theta}}_{N_{\theta}}=\boldsymbol{\mathbf{r}}_{f}$
$\displaystyle\sum_{i=0}^{N_{\theta}}\boldsymbol{\mathbf{\Theta}}_{i}\boldsymbol{\mathbf{b}}^{T}_{1,i+1}\boldsymbol{\mathbf{\Lambda}}_{d_{\theta}-1}(0)=\overline{v}_{\theta}\begin{bmatrix}\cos\psi_{0}\\\
\sin\psi_{0}\end{bmatrix},$
$\displaystyle\sum_{i=0}^{N_{\theta}}\boldsymbol{\mathbf{\Theta}}_{i}\boldsymbol{\mathbf{b}}^{T}_{1,i+1}\boldsymbol{\mathbf{\Lambda}}_{d_{\theta}-1}(1)=\overline{v}_{\theta}\begin{bmatrix}\cos\psi_{f}\\\
\sin\psi_{f}\end{bmatrix},$
$\displaystyle\bigg{\|}\sum_{i=0}^{N_{\theta}}\boldsymbol{\mathbf{\Theta}}_{i}b_{1,i+1,j+1}\bigg{\|}_{2}\leq\overline{v}_{\theta},j=1,\ldots,N_{\theta},$
(28) and (29) hold

with decision variables
$\beta,\overline{v}_{\theta},\underline{v}_{\theta},\overline{a}_{\theta}$ and
$,\boldsymbol{\mathbf{\Theta}}_{0},\ldots,\boldsymbol{\mathbf{\Theta}}_{N_{\theta}}$,
[XX: what’s decision variable
$\boldsymbol{\mathbf{\Theta}}_{0},\ldots,\boldsymbol{\mathbf{\Theta}}_{N_{\theta}}$?][VF:
See Definition 2] where $\overline{\gamma}$ is the maximum steering angle,
$\boldsymbol{\mathbf{r}}_{m}\triangleq(x_{m},y_{m})^{T},\;m\in\\{0,f\\},$ is
the initial/final position vector and $\psi_{m}$ is the initial/final heading
angle. The problem (PATH-SOCP) is convex and can be solved efficiently with
commercial solvers such as MOSEK [mosek].

We call a B-spline path whose parameters are feasible points of (PATH-SOCP) a
_safe B-spline path_.

######  Remark 2.

The B-spline path $\boldsymbol{\mathbf{\theta}}(s)$ can be obtained by using
the solution of (PATH-SOCP) by (6) and Definition 2. This curve, along with
the speed profile, whose construction will be discussed in Section IV-C,
defines a parameterization of the flat outputs (17). Note that the B-spline
path $\boldsymbol{\mathbf{\theta}}$ obtained from (PATH-SOCP) has bounded
derivatives given by $\overline{v}_{\theta}$ and $\overline{a}_{\theta}$ which
will be used to guarantee the safety of speed profiles in Section IV-C.

######  Remark 3.

Proposition 2 contains two relaxations of nonconvex constraints: Lower-
bounding a norm and lower-bounding a convex quadratic function. Because of the
relaxations, the size of the feasible region of (PATH-SOCP) depends on the
choice of parameters $\alpha$ and $\hat{\boldsymbol{\mathbf{r}}}$. The
following heuristic worked well in practice for simple motions:

$\alpha=\frac{2\tan\overline{\gamma}}{L}\|\boldsymbol{\mathbf{r}}_{f}-\boldsymbol{\mathbf{r}}_{0}\|_{2},\quad\hat{\boldsymbol{\mathbf{r}}}=\frac{\boldsymbol{\mathbf{r}}_{f}-\boldsymbol{\mathbf{r}}_{0}}{\|\boldsymbol{\mathbf{r}}_{f}-\boldsymbol{\mathbf{r}}_{0}\|_{2}}.$

###  IV-B Temporal Optimization

In this subsection, we find an appropriate trajectory duration [XX: call it
duration or termination time or something else?] [VF: It is consistently
called the “duration” referring to the trajectory’s duration.] $t_{f}$ by
considering a minimization of both $t_{f}$ and the magnitude of the
acceleration vector.

Consider a safe B-spline path $\boldsymbol{\mathbf{\theta}}(s)$, and the
following functions

$b(s)\triangleq\dot{s}^{2},\quad a(s)\triangleq\ddot{s},$ (30)

which must satisfy the differential condition:

$b^{\prime}(s)=2a(s).$ (31)

Following [verscheure2009time], we have that

$t_{f}=\int_{0}^{t_{f}}1\;\mathrm{d}t=\int_{0}^{1}\frac{1}{\dot{s}}\;\mathrm{d}s=\int_{0}^{1}b(s)^{-1/2}\;\mathrm{d}s.$
(32)

Purely minimizing the trajectory duration given by $t_{f}$ results in maximum-
speed velocity profiles. We additionally minimize the acceleration to
encourage trajectories with mild friction circle profiles
[rajamani2011vehicle] by choosing Lagrange cost functional:

$L\big{(}\boldsymbol{\mathbf{x}}(t),\boldsymbol{\mathbf{u}}(t)\big{)}\triangleq\dot{v}^{2}(t)+v^{2}(t)\dot{\psi}^{2}(t)=\|\ddot{\boldsymbol{\mathbf{y}}}(t)\|_{2}^{2}.$
(33)

We can now write the objective functional entirely in terms of (and convex
with respect to) the new functions $a(s)$ and $b(s)$ as follows:

$J\big{(}a(s),b(s)\big{)}=\int_{0}^{1}\frac{\nu}{\sqrt{b(s)}}+\|a(s)\boldsymbol{\mathbf{\theta}}^{\prime}(s)+b(s)\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s)\|^{2}_{2}\;\mathrm{d}s.$
(34)

We follow a similar procedure as in [verscheure2009time] and consider
$N_{t}+1$ points partitioning the interval $[0,1]$ into $N_{t}$ uniform
segments with width $\Delta s\triangleq 1/N_{t}$. The discretized Lagrange
cost functional becomes:

$L(s_{i},a_{i},b_{i})=\|a_{i}\boldsymbol{\mathbf{\theta}}^{\prime}(s_{i})+b_{i}\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s_{i})\|^{2}_{2},$

where $a_{i}$ and $b_{i}$ are the decision variables representing $a(s_{i})$
and $b(s_{i})$, respectively. Assuming that $a(s)$ is piece-wise constant over
each segment $[s_{i},s_{i+1}),i=0,\ldots,N_{t}-1$ where $s_{i}\triangleq
i\Delta s$, we can exactly evaluate the integral (32) to avoid the case when
$\dot{s}=0$ as shown in [verscheure2009time].

We formulate the following SOCP to obtain the trajectory duration $t_{f}$.

min. $\displaystyle\quad\sum_{i=0}^{N_{t}-1}2\nu\Delta
sd_{i}+\sum_{i=0}^{N_{t}}L(s_{i},a_{i},b_{i})$ (TIME-SOCP) s. t.
$\displaystyle\Bigg{\|}\begin{bmatrix}2c_{i}\\\
b_{i}-1\end{bmatrix}\Bigg{\|}_{2}\leq b_{i}+1,\quad i=0,\ldots,N_{t},$
$\displaystyle\Bigg{\|}\begin{bmatrix}2\\\ \Delta
c_{i}-d_{i}\end{bmatrix}\Bigg{\|}_{2}\leq\Delta
c_{i}+d_{i},\;i=0,\ldots,N_{t}-1,$ $\displaystyle\Delta c_{i}=c_{i+1}+c_{i},$
$\displaystyle 2\Delta sa_{i}=b_{i}-b_{i-1},\quad i=1,\ldots,N_{t},$
$\displaystyle
b_{0}\|\boldsymbol{\mathbf{\theta}}^{\prime}(0)\|_{2}^{2}=v_{0}^{2},\quad
b_{N_{t}}\|\boldsymbol{\mathbf{\theta}}^{\prime}(1)\|_{2}^{2}=v_{f}^{2},$
$\displaystyle
b_{i}\|\boldsymbol{\mathbf{\theta}}^{\prime}(s_{i})\|_{2}^{2}\leq\overline{v}^{2},\quad
i=0,\ldots,N_{t},$
$\displaystyle\Big{|}a_{i}\|\boldsymbol{\mathbf{\theta}}^{\prime}(s_{i})\|_{2}+b_{i}f_{i}\Big{|}\leq\overline{a},\quad
i=0,\ldots,N_{t},$ $\displaystyle
f_{i}=\frac{\Big{(}\boldsymbol{\mathbf{\theta}}^{\prime}(s_{i})\cdot\boldsymbol{\mathbf{\theta}}^{\prime\prime}(s_{i})\Big{)}}{\|\boldsymbol{\mathbf{\theta}}(s_{i})\|_{2}},$

with decision variables $a_{i},b_{i},c_{i},d_{i}$, $i=0,\ldots,N_{t}$, where
the first three constraints are the SOCP embedding of (32) given in
[verscheure2009time], the fourth constraint is from (31), the fifth sets
initial and final speeds, the sixth ensures the speed bound is respected and
the seventh ensures the acceleration bound is respected. From our assumption
that the function $a(s)$ is constant over each $[s_{i},s_{i+1})$ segment, we
can recover the duration of each segment from the constant acceleration
equation:

$\Delta t_{i}=\frac{\sqrt{b_{i}}+\sqrt{b_{i-1}}}{a_{i}},\quad
i=1,\ldots,N_{t}.$ (35)

The overall duration of the trajectory is then

$t_{f}=\sum_{i=1}^{N_{t}}\Delta t_{i}.$ (36)

######  Remark 4.

The solution of (TIME-SOCP) provides a safe speed profile at discrete time
instances. If continuous-time safety is not critical, it suffices to stop here
and retrieve the discretized state-space solution. In addition, if the desired
trajectory duration $t_{f}$ is known, one can skip (TIME-SOCP) and proceed to
the next SOCP after solving (PATH-SOCP).

###  IV-C Speed Profile Optimization

Assume that $t_{f}$ is given by the solution of (TIME-SOCP) or specified a
priori. Let $\boldsymbol{\mathbf{\theta}}(s)$ be a safe B-spline path (i.e.,
feasible in (PATH-SOCP)) and $s(t)$ be a B-spline speed profile.

######  Proposition 4.

If the condition

$0\leq\sum_{i=0}^{N_{s}}\overline{v}_{\theta}p_{j}b_{1,i+1,j+1}\leq\overline{v},\quad
j=1,\ldots,N_{s},$ (37)

holds, then the state-space trajectory
$\boldsymbol{\mathbf{x}}(t)\in\mathcal{S}_{v},$ $\forall t\in[0,t_{f})$ where
$\mathcal{S}_{v}$ is the _forward speed safety set_ defined in (24).

###### Proof.

By Proposition 1, (37) implies that
$0\leq\overline{v}_{\theta}\dot{s}(t)\leq\overline{v}$ for all
$t\in[0,t_{f})$. Because

$\overline{v}\geq\overline{v}_{\theta}\dot{s}(t)\geq\dot{s}(t)\|\boldsymbol{\mathbf{\theta}}\big{(}s(t)\big{)}\|_{2}=v(t)\geq
0,$

the conclusion follows. ∎

######  Proposition 5.

For any given nonnegative vectors
$\overline{\boldsymbol{\mathbf{m}}}=(\overline{m}_{0},\ldots,\overline{m}_{N_{s}-d_{s}})^{T},\;m\in\\{\kappa,\epsilon\\},$
if the following condition

$\displaystyle
0\leq\sum_{i=0}^{N_{s}}p_{j}b_{1,i+1,j+1}\leq\overline{\kappa}_{k},\quad
j=k+1,\ldots,k+d,$ (38a)
$\displaystyle-\overline{\epsilon}_{k}\leq\sum_{i=0}^{N_{s}}p_{j}b_{2,i+1,j+1}\leq\overline{\epsilon}_{k},\;j=k+2,\ldots,k+d,$
(38b)
$\displaystyle\bigg{\|}A_{\dot{v}}\begin{bmatrix}\overline{\kappa}_{k}\\\
\overline{\epsilon}_{k}\end{bmatrix}+\boldsymbol{\mathbf{b}}_{\dot{v}}\bigg{\|}_{2}\leq\boldsymbol{\mathbf{c}}_{\dot{v}}^{T}\begin{bmatrix}\overline{\kappa}_{k}\\\
\overline{\epsilon}_{k}\end{bmatrix}+d_{\dot{v}},$ (38c)

holds for all $k\in\\{0,\ldots,N_{s}-d_{s}\\}$, where

$\displaystyle A_{\dot{v}}$
$\displaystyle=\begin{bmatrix}\sqrt{2\overline{a}_{\theta}}&0\\\
0&-\frac{\overline{v}_{\theta}}{\sqrt{2}}\end{bmatrix},\quad\boldsymbol{\mathbf{b}}_{\dot{v}}=\begin{bmatrix}0\\\
\frac{\overline{a}-1}{\sqrt{2}}\end{bmatrix},$
$\displaystyle\boldsymbol{\mathbf{c}}_{\dot{v}}$
$\displaystyle=\begin{bmatrix}0\\\
-\frac{\overline{v}_{\theta}}{\sqrt{2}}\end{bmatrix},\quad
d_{\dot{v}}=\frac{\overline{a}+1}{\sqrt{2}},$

then the state-space trajectory
$\boldsymbol{\mathbf{u}}(t)\in\mathcal{S}_{\dot{v}}$ for all $t\in[0,t_{f})$
where $\mathcal{S}_{\dot{v}}$ is the _linear acceleration safety set_ defined
in (25).

###### Proof.

The first two conditions imply, by Proposition 1, that for any
$k\in\\{0,\ldots,N_{s}-d_{s}\\}$

$\displaystyle 0\leq\dot{s}(t)$
$\displaystyle\leq\overline{\kappa}_{k},\quad\forall
t\in[\tau_{k+d_{s}},\tau_{k+d_{s}+1}),$ $\displaystyle|\ddot{s}(t)|$
$\displaystyle\leq\overline{\epsilon}_{k},\quad\forall
t\in[\tau_{k+d_{s}},\tau_{k+d_{s}+1}).$

Expanding the last condition, we determine that for any
$k\in\\{0,\ldots,N_{s}-d_{s}\\}$,

$0\leq\overline{\kappa}^{2}_{k}\overline{a}_{\theta}+\overline{\epsilon}_{k}\overline{v}_{\theta}=|\overline{\kappa}^{2}_{k}\overline{a}_{\theta}|+|\overline{\epsilon}_{k}\overline{v}_{\theta}|\leq\overline{a}.$

Now, notice that for any $k\in\\{0,\ldots,N_{s}-d_{s}\\}$, the conditions:

$\displaystyle|\overline{\epsilon}_{k}\overline{v}_{\theta}|$
$\displaystyle\geq|\ddot{s}(t)|\|\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\|_{2}=|a_{t}(t)|,$
$\displaystyle|\overline{\kappa}_{k}^{2}\overline{a}_{\theta}|$
$\displaystyle\geq\Bigg{|}\frac{\dot{s}^{2}(t)\Big{(}\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\cdot\boldsymbol{\mathbf{\theta}}^{\prime\prime}\big{(}s(t)\big{)}\Big{)}}{\|\boldsymbol{\mathbf{\theta}}^{\prime}\big{(}s(t)\big{)}\|}\Bigg{|}=|a_{n}(t)|,$

where $a_{t}$ and $a_{n}$ are defined in (27), in hold for all
$t\in[\tau_{k+d_{s}},\tau_{k+d_{s}+1})$. Therefore, by the triangle inequality
we have that:

$\overline{a}\geq|a_{t}(t)|+|a_{n}(t)|\geq|a_{t}(t)+a_{n}(t)|,$

holds for all $t\in[\tau_{k+d_{s}},\tau_{k+d_{s}+1})$. Recalling that the knot
vector $\boldsymbol{\mathbf{\tau}}$ is clamped and uniform (5) segmenting the
interval $[0,t_{f}]$, and that the above inequality holds for all
$k\in\\{0,\ldots,N_{s}-d_{s}\\}$, we can establish that the inequality holds,
in fact, for all $t\in[0,t_{f})$. The conclusion now follows directly by Lemma
4. ∎

We formulate the following SOCP to obtain the speed profile.

$\displaystyle\min\limits_{\overline{\boldsymbol{\mathbf{\kappa}}},\overline{\boldsymbol{\mathbf{\epsilon}}},p_{0},\ldots,p_{N_{s}}}\quad$
$\displaystyle\int_{0}^{t_{f}}\dddot{s}^{2}(t)\;\mathrm{d}t$ (SPEED-SOCP) s.
t. $\displaystyle p_{0}=0,\quad p_{N_{s}}=1,$
$\displaystyle\sum_{i=0}^{N_{s}}\overline{v}_{\theta}p_{i}\boldsymbol{\mathbf{b}}_{1,i+1}^{T}\boldsymbol{\mathbf{\Lambda}}_{d_{s}-1}(0)=v_{0},$
$\displaystyle\sum_{i=0}^{N_{s}}\overline{v}_{\theta}p_{i}\boldsymbol{\mathbf{b}}_{1,i+1}^{T}\boldsymbol{\mathbf{\Lambda}}_{d_{s}-1}(t_{f})=v_{f},$
$\displaystyle\mbox{\text{\eqref{cvx_v_cst} and \eqref{cvx_v_dot_cst} hold}},$

where $v_{0}$ and $v_{f}$ are specified initial and final speeds,
respectively.

We call a B-spline speed profile whose parameters are feasible points of
(SPEED-SOCP) a safe B-spline speed profile.

###  IV-D Safety Analysis

The following theorem summarizes the main theoretical contributions of this
paper.

######  Theorem 1.

Let $\boldsymbol{\mathbf{\theta}}(s)$ be a safe B-spline path feasible in
(PATH-SOCP) and $s(t)$ be a safe B-spline speed profile feasible in (SPEED-
SOCP) with duration $t_{f}$. The corresponding state-space trajectory of (1)
obtained by passing the parameterized flat outputs (17) in terms of
$\boldsymbol{\mathbf{\theta}}$ and $s$ through the flat map (4) satisfies the
initial and final conditions:

$\boldsymbol{\mathbf{x}}(0)=\boldsymbol{\mathbf{x}}_{0},\quad\boldsymbol{\mathbf{x}}(t_{f})=\boldsymbol{\mathbf{x}}_{f},$
(39)

as well as safety specifications:

$\displaystyle\big{(}\boldsymbol{\mathbf{x}}(t),\boldsymbol{\mathbf{u}}(t)\big{)}$
$\displaystyle\in\mathcal{S}_{\gamma},$ (40a)
$\displaystyle\boldsymbol{\mathbf{x}}(t)$
$\displaystyle\in\mathcal{S}_{\mathcal{D}},$ (40b)
$\displaystyle\boldsymbol{\mathbf{x}}(t)$ $\displaystyle\in\mathcal{S}_{v},$
(40c) $\displaystyle\boldsymbol{\mathbf{u}}(t)$
$\displaystyle\in\mathcal{S}_{\dot{v}},$ (40d)

for all $t\in[0,t_{f})$. Thus, $t_{f}$, $\boldsymbol{\mathbf{x}}(t)$ and
$\boldsymbol{\mathbf{u}}(t)$ are a feasible point of (OPT).

###### Proof.

The initial and final positions $(x,y)$ are satisfied because
$\boldsymbol{\mathbf{\theta}}(s)$ satisfies
$\boldsymbol{\mathbf{\theta}}(0)=\boldsymbol{\mathbf{r}}_{0}=(x_{0},y_{0})^{T}$
and
$\boldsymbol{\mathbf{\theta}}(1)=\boldsymbol{\mathbf{r}}_{f}=(x_{f},y_{f})^{T}$.
Furthermore, $\|\boldsymbol{\mathbf{\theta}}(s)\|_{2}=\overline{v}_{\theta}$
for $s\in\\{0,1\\}$. Then, since $s(0)=0$, $s(t_{f})=1$,
$\overline{v}_{\theta}\dot{s}(0)=v_{0}$ and
$\overline{v}_{\theta}\dot{s}(t_{f})=v_{f}$, we have by the flat output
parameterization (17a) and the flat map (4) that the obtained state-space
trajectory $\boldsymbol{\mathbf{x}}(t)$ satisfies the specified initial and
final velocities $v_{0}$ and $v_{f}$. For the initial and final heading angles
$\psi_{0}$ and $\psi_{f}$, notice that

$\boldsymbol{\mathbf{\theta}}^{\prime}(m)=\begin{bmatrix}x^{\prime}(m)\\\
y^{\prime}(m)\end{bmatrix}=\overline{v}_{\theta}\begin{bmatrix}\cos\psi_{q}\\\
\sin\psi_{q}\end{bmatrix},\;(m,q)\in\\{(0,0),(1,f)\\}.$

Since the state $\psi(t)$ trajectory is given only by the path
$\boldsymbol{\mathbf{\theta}}(s)$ under the considered parameterization (17),
it follows that:

$\psi(t_{q})=\arctan\Bigg{(}\frac{\overline{v}_{\theta}\sin\psi_{q}}{\overline{v}_{\theta}\cos{\psi_{q}}}\bigg{)}=\psi_{q},\quad
q\in\\{0,f\\}.$

The safety specifications (40) follow directly from Propositions 2, 3, 4 and
5. ∎

Table II provides a summary of the presented Lemmas and Propositions with
their respective safety sets.

TABLE II: Proposed framework safety guarantees. Safety Set | Necessary and Sufficient | Sufficient & Convex
---|---|---
$S_{\gamma}$ (19) | Lemma 1 | Proposition 2
$S_{D}$ (20) | Lemma 2 | Proposition 3
$S_{v}$ (24) | Lemma 3 | Proposition 4
$S_{\dot{v}}$ (25) | Lemma 4 | Proposition 5

##  V Examples & Performance

In this section, we evaluate the proposed framework’s performance and
efficiency. First, we formulate an instance of (OPT) and use the proposed
framework to find a feasible solution, we use plots to demonstrate the safety
guarantees. Next, we compare the proposed framework’s solve speed and
optimality to that of state-of-the-art optimal control solvers. Finally, we
show that the framework can be used for real-time navigation with a MATLAB
simulation example in Mcity. In all examples, we use B-spline path
$\boldsymbol{\mathbf{\theta}}$ parameters $d_{\theta}=4$ and $N_{\theta}=20$,
and B-spline speed profile $s$ parameters $d_{s}=4$ and $N_{s}=20$. For (TIME-
SOCP), we use $N_{t}=40$ uniform segments to partition the interval $[0,1]$.

###  V-A Safety Guarantees Example

Let us consider (OPT) with the following problem data as described in Table I:
Initial state $\boldsymbol{\mathbf{x}}_{0}=\boldsymbol{\mathbf{0}}_{4\times
1}$, final state $\boldsymbol{\mathbf{x}}_{f}=(100,4,0,0)^{T}$, obstacle-free
space $\mathcal{D}=\mathbb{R}^{2}$, maximum steering angle
$\overline{\gamma}=0.0044$ rad ($0.25$ degrees), maximum speed
$\overline{v}=4.2$ m/s, maximum acceleration and braking $\overline{a}=0.6$
m/s2, duration penalty factor $\nu=1$ and wheelbase length $L=2.601$ meters.
Note that this problem requires rest-to-rest motion to be solved and that, as
described in previous sections, the flat map has singularities when the speed
is zero. However, our approach is able to handle this gracefully. We continue
by solving the three proposed SOCP problems sequentially, as shown in Figure
1, using YALMIP [Lofberg2004] with MOSEK [mosek]. We then pass the resulting
path and speed profile through the flat map (4) to obtain corresponding state
$\boldsymbol{\mathbf{x}}(t)$ and input $\boldsymbol{\mathbf{u}}(t)$
trajectories. The resulting state trajectory is shown in Figure 3(a). It can
be seen that the speed $v(t)\leq\overline{v}$ for all time $t\in[0,t_{f}]$.
The resulting input trajectory along with the steering angle $\gamma(t)$ are
shown in Figure 3(b). We can also see that the safety constraints are
satisfied. In particular, we have that $|\dot{v}(t)|\leq\overline{a}$ and
$|\gamma(t)|\leq\overline{\gamma}$ for all time $t\in[0,t_{f}]$.

(a) State trajectory. The speed respects $v(t)\leq 4.2$ m/s.

(b) Input trajectory and steering angle $\gamma(t)$. The acceleration respects
$|\dot{v}(t)|\leq 0.6$ m/s2 and the steering angle respects $|\gamma(t)|\leq
0.25$ deg.

Figure 3: Verification that the safety constraints for state and inputs are
satisfied.

###  V-B Performance Comparison

In this section we compare the performance and optimality of the proposed
framework with state-of-the-art optimal control solvers: ICLOCS2
[nie2018iclocs2] and OpenOCL [koenemann2017openocl]. For our framework, we
solve the three SOCP using YALMIP [Lofberg2004] with MOSEK [mosek]. The
vehicle considered is a 2021 Bolt EV by Chevrolet with a wheelbase length of
$L=2.601$ meters. We let the duration penalty factor $\nu=1$. We assume a
typical 2-lane, straight, road with a posted speed limit of 40 miles per hour
and consider a left lane change. The initial and final states are specified as
$\boldsymbol{\mathbf{x}}_{0}=(0,0,16,0)^{T}$ and
$\boldsymbol{\mathbf{x}}_{f}=(75,3.7,17.5,0)^{T}$, respectively. The other
parameters are chosen as $\overline{\gamma}=0.785$ ($45$ degrees),
$\mathcal{D}=\mathbb{R}^{2}$, $\overline{v}=19$ m/s, $\overline{a}=2$ m/s2 and
$\nu=1$. With these parameters, we solve the optimal control problem (OPT)
with the proposed framework, ICLOCS2 with analytical derivatives information
provided and 40 discretization samples, and OpenOCL with the default
configuration and 40 discretization samples. The resulting trajectories
$\boldsymbol{\mathbf{x}}(t)$ and $\boldsymbol{\mathbf{u}}(t)$ are shown in
Figure 4. To compare the computational burden of each algorithm, we collect an
average solve time of 50 runs with each approach. The results are shown in
Table III. In this example, the proposed approach achieve solve times nearly
four times faster than the next leading method. In addition, the objective
value of the proposed approach, while higher, is still comparable to that of
the other methods. In particular, we can determine from the plots of
$\dot{v}(t)$ and $\dot{\psi}(t)$ in Figure 4 that the proposed approach
produces trajectories that require more aggressive accelerations but have
lower curvature. This discrepancy in the objective values is due to the
separation of the original objective function across three different
optimization problems.

Figure 4: Lane change example. We solve (OPT) with the proposed framework and compare the obtained state-space trajectory with state-of-the-art solvers. The top figure shows the lane changing maneuver obtained by each solver. They are similar in all three cases. The bottom plots show the state and input trajectories obtained with each solver. TABLE III: Comparison of efficiency and optimality of the proposed approach with state of the art solvers. |  Avg solve time [ms] |  Objective value [-]
---|---|---
Proposed | 28.8 | 6.8495
ICLOCS2 | 257 | 6.8134
OpenOCL | 94.3 | 6.5534

###  V-C Mcity Navigation Example

Figure 5: Navigation example using the proposed framework to solve (OPT) at 25
Hz. The figure shows the scenario at times $t=0,2,4,6,8$ seconds.

We demonstrate the real-time capabilities of the framework in a simulated
scenario located at Mcity’s main roundabout. The vehicle begins from rest at
the roundabout entrance and must adjust to existing traffic and take the
roundabout’s second exit. We enforce the constraint
$v(t)\leq\overline{v}=11.176$ m/s to conform to typical 25 mph speed limits in
residential areas. We also consider two actors driving around the roundabout
with constant speed of $5$ m/s. The adjustment to traffic is done by simple
behavioral logic as follows: If the planned trajectory, with desired final
position $\boldsymbol{\mathbf{r}}_{f}$ located 15 meters ahead on the road, is
obstacle-free, it is used. Otherwise, if it collides with the vehicle in
front, we adjust the endpoint of the trajectory to be $8$ meters behind the
leading vehicle (but still on the road’s center line) and enforce a final
speed $v_{f}=2.5$ m/s. The proposed framework is solved at 25 Hz and, for each
solution, the corresponding control is applied. Thus, we achieve similar
behavior as that of model predictive control with finite horizon.
Specifically, once the input trajectory $\boldsymbol{\mathbf{u}}(t)$ is found
by the proposed framework, we apply $\boldsymbol{\mathbf{u}}(0.04)$ (25 Hz) to
the vehicle. The scenario is rendered in MATLAB’s 3D simulation environment
powered by Unreal Engine. We show snapshots of the trajectory at five
different time steps in Figure 5. The figure also shows a bird’s eye view of
the scenario and the planned trajectory at the current time step (gray line).

##  VI Conclusions

We presented a framework based on convex optimization that finds state and
input vehicle trajectories which are guaranteed to be feasible for all times,
but generally sub-optimal, in a constrained optimal control problem. We also
compared the performance of the proposed framework to that of current state-
of-the-art solvers and demonstrated its efficiency in a simulated scenario. In
future work, we will leverage the safety guarantees of the proposed framework
to achieve safety both at a higher level in the form of objective success and
at a lower level in the form of tracking safety guarantees.