# Trajectory Generation for Autonomous Vehicles Using Atropoids

Alexander Botros and Stephen L. Smith This work is supported in part by the
Natural Sciences and Engineering Research Council of Canada (NSERC)The authors
are with the Department of Electrical and Computer Engineering, University of
Waterloo, 200 University Ave W, Waterloo ON, Canada, N2L 3G1
<EMAIL_ADDRESS><EMAIL_ADDRESS>
# Comfortable, Tunable Trajectory Generation For Autonomous Vehicles Using
$G3$ Curves

Alexander Botros and Stephen L. Smith This work is supported in part by the
Natural Sciences and Engineering Research Council of Canada (NSERC)The authors
are with the Department of Electrical and Computer Engineering, University of
Waterloo, 200 University Ave W, Waterloo ON, Canada, N2L 3G1
<EMAIL_ADDRESS><EMAIL_ADDRESS>
##  I Introduction

In motion planning for mobile robots and autonomous vehicles, a trajectory
generation technique is commonly used as part of a local planner
[urmson2008autonomous]. The local planner produces many local trajectories
satisfying constraints that reflect the vehicles dynamics, and discards those
trajectories that collide with static or predicted dynamic obstacles. The best
trajectory is selected according to a metric that captures the
efficiency/length of the trajectory, and its smoothness or comfort
[paden2016survey]. The trajectory is used as the reference for a tracking
controller for a fixed amount of time, before the local planner produces a new
reference.

In this paper, we address the problem of generating a trajectory for a mobile
robot or autonomous vehicle from a start state to a goal state. We focus on
states of the form $(x,y,\theta,\kappa,v)$ where $(x,y)\in\mathbb{R}^{2}$
represents the planar location of the vehicle, $\theta$ the heading, $\kappa$
the curvature, and $v$ the velocity. In particular, we focus on computing
trajectories that optimize a trade off between comfort and travel time
[levinson2011towards, ziegler2014trajectory].

Comfort of a trajectory is related to the acceleration, rate of change of yaw,
and jerk (i.e., the derivative of the acceleration) experienced by a particle
(vehicle) sliding along it [ziegler2014trajectory], [huang2004fundamental],
[articlejerk]. A common metric for comfort is the integral of the square of
the jerk (IS jerk) over the trajectory [levinson2011towards]. When a vehicle
is moving at constant speed, jerk is experienced lateral to the trajectory and
is directly proportional to the derivative of curvature—called the _sharpness_
—of the vehicle trajectory. It is important to note that the smoothness and
comfort of the resulting vehicle motion depends on both the reference
trajectory and the tracking controller. However, by planning trajectories with
low jerk, less effort is required from the controller to track the reference
and ensure comfort [kyriakopoulos1994minimum], particularly at high speeds
[shin1992path].

Often, trajectory planning is treated as a two part spatio-temporal problem
[katrakazas2015real], sometimes with feedback between both sub-problems
[kant1986toward]. The first sub-problem deals with computing a _path_ in the
states $(x,y,\theta,\kappa,\sigma)$ from start to goal. In this paper, we will
refer to these as _path states_ , while the second sub-problem addresses how
this path should be converted into a motion by computing a velocity profile.
The feedback between these two sub-problems ensures the trajectory’s adherence
to desirable properties like short travel time, and comfort.

In this paper, we therefore address the problems of path and velocity planning
as separate but coupled problems. We present an algorithm that computes a path
with continuous curvature rate from a starting state to a goal state up to
unknown parameters. We then use a well known cost function that trades off
comfort and travel time to determine these parameters as well as a velocity
profile. Analysis of the cost function using Pontryagin’s Minimum Principle
(PMP) is employed to develop relations between velocity and path parameters.

Figure 1: Example reference paths generated using $G3$ curves.

###  I-A Contributions

This paper contributes to the work of trajectory planning in the following
ways:

  * •

We present an algorithm to compute paths with continuous curvature rate
between start and goal states $p_{s},p_{g}$. This algorithm takes as input,
bounds on the magnitude of curvature ($\kappa_{\text{max}}$), curvature rate
($\sigma_{\text{max}}$), and curvature acceleration ($\rho_{\text{max}}$), as
well as the states $p_{s},p_{g}$. This algorithm is minimizes the final arc-
length of the path provided that the start and goal states are sufficiently
far apart.

  * •

We propose a method by which the bounds
$\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ as well as the
velocity profile, $v$, can be approximated. This is done by minimizing a cost
function that penalizes discomfort and final arc-length while assuming a
simplistic form a the velocity profile.

  * •

As noted in [CITE ORIGINAL SPLINE PAPER], a major source of computational
burden in path planning algorithms lies in the evaluation of Fresnel
integrals. For this reason, a fast method to approximate such integrals is
included in the Appendix.

###  I-B Related Work

####  I-B1 Path Planning

Path planning techniques can typically be divided into two categories: a path-
fitting approach, and a curvature fitting approach. In the path fitting
approach, a form of the path is fixed up to unknown parameters. These
parameters are then computed numerically by solving optimization problems.
Example path-forms include $G2-$splines [inproceedings2, inproceedings], and
Bezier curves [5233184, 5548085]. In the path fitting approach, smoothness and
comfort of paths is typically enforced by minimizing the maximum curvature
rate (the rate of change of curvature with respect to arc-length) over the
trajectory. Minimizing the maximum curvature rate as opposed to simply
bounding it, may result in longer than necessary trajectories.

Unlike the path fitting approach, the curvature fitting approach assumes a
form of the curvature profile over the path (as opposed to the path itself) up
to unknown parameters. The unknown parameters are then computed by solving a
two point boundary value problem between start and goal configurations. Early
work in this approach includes Dubins’ paths [Dubins_1957], which are the
shortest paths between start and goal configurations if the curvature is
bounded.

In [Kelly-2003-8714, 6225063], the authors assume that the desired curvature
is a cubic polynomial. However, the resulting path does not possess sufficient
degrees of freedom to account for bounds on the curvature, resulting in
potentially infeasible paths that violate the physical curvature constraints
of the vehicle.

Clothoid paths [1362698], on the other hand, address feasibility issues by
placing constraints on both curvature and curvature rate over the entire path
resulting in paths with piece-wise linear curvature rate. A _clothoid_ , or
Euler Spiral, is a curve whose instantaneous curvature, $\kappa$ is a linear
function of the curve’s arc-length, $s$. When the start and goal
configurations are given as an $(x,y)$ pose, a heading $\theta$, and a
curvature $\kappa$, several methods have been proposed to generate the
shortest path from start to goal as a sequence of clothoid and straight line
segments [shin1992path], [Gim2017], [8461201]. However, since the curvature of
a clothoid varies linearly with arc length, the sharpness of the path (and
thus the lateral jerk) is constant. Hence, a clothoid is a path that minimizes
the maximum squared jerk rather than the IS jerk.

Requiring that the curvature be twice differentiable, and placing bounds on
curvature, curvature rate, and curvature acceleration results in paths called
$G3$ paths whose IS jerk (and not simply maximum squared jerk) can be
minimized. Such paths are to clothoid ($G2$) paths, what clothoids were to
Dubin’s ($G1$) paths, piece-wise linear curvature). Thus $G3$ paths are one
step closer to paths with infinitely differentiable curvatures – like spline
and Bezier paths – while minimizing arc-length in the presence of explicit
restrictions on curvature, and curvature rate (like clothoid paths). In
[oliveira2018trajectory], the authors obtain $G3$ paths by concatenating cubic
spline curves and straight lines. Though this approach results in $G3$ paths,
the spline segments of the path, which are infinitely differentiable, may
result in longer than necessary arc-lengths as is the case with regular spline
paths.

In [banzhaf2018g], the authors introduce continuous curvature rate (CCR), and
hybrid curvature rate (HCR) curves. These curves are $G3$ curves in which the
curvature rate, $\sigma$, is a piece-wise linear function of the arc-length,
$s$, and the curvature acceleration, $\rho$, is piece-wise constant. Example
$G3$ curves are given in Figure 1. The authors of [banzhaf2018g] compute paths
between start and goal states by combining CCR/HCR curves and straight lines.
Unlike the techniques employed in [oliveira2018trajectory], these curves are
not infinitely differentiable as their curvature acceleration is
discontinuous.

By placing bounds on curvature rate, and curvature acceleration, paths
developed by joining HCR/CCR curves and straight lines have the added benefit
of bounding not only lateral/longitudinal jerk, but angular jerk (the rate of
change of angular acceleration), as well as lateral/longitudinal snap (the
derivative of jerk). These benefits make HCR/CCR curves particularly
attractive for use in the path planning phase of trajectory planning. The
techniques developed in [banzhaf2018g] will be extended in this paper in four
ways. First, the authors of [banzhaf2018g] focus on paths whose start and goal
states have either 0 or maximum curvature. Though the symmetry resulting from
this assumption greatly simplifies the mathematics of path planning, it limits
the usefulness of the approach in producing a tracking trajectory. In
particular, while tracking a reference trajectory, it may occur that the
vehicle slips off of the planned path and requires a re-plan from a state
whose curvature is not 0 or its maximum value. It is for this reason that the
work herein relaxes the assumptions on the initial and final curvatures.
Second, the authors of [banzhaf2018g] use search techniques similar to $A^{*}$
to join HCR/CCR curves with straight lines to produce a path. In this paper,
we present an algorithm to quickly determine such paths by reducing the path
planning problem to a Dubin’s-like sub-problem. Third, it is assumed in
[banzhaf2018g], that the maximum values of curvature ($\kappa_{\text{max}}$),
curvature rate ($\sigma_{\text{max}}$), and curvature acceleration
($\rho_{\text{max}}$) are known. However, these bounds should be functions of
speed as, for example, a driver may turn the steering wheel at their maximum
rate in a parking lot, but not on a highway. Thus, in this paper, we develop
means by which $\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ can
be computed by taking the situation into account. This is done by determining
bounds that minimize a trade off between comfort and travel time. Finally, it
is assumed in [banzhaf2018g], that the velocity of the vehicle is a positive
or negative constant. While the work presented herein is limit to positive
speeds, we develop a velocity profile that optimizes a tradeoff between
comfort and path arc-length. In particular, we investigate the influence of
$\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ and velocity on
comfort and arc-length, and select optimizing values.

####  I-B2 Velocity Planning

Once a candidate path has been developed, a velocity profile to describe the
motion of the vehicle along that path must be computed. This velocity profile
should take into account a tradeoff between comfort and duration of travel. A
common technique involves minimizing a cost function that is a weighted sum of
undesirable features [levinson2011towards]. In [ziegler2014trajectory,
ziegler2014making], the authors integrate the weighted sum of the squared
offset of the trajectory from the center of the road, the squared error in
velocity from a desired profile, the squared acceleration, the squared jerk,
and the squared yaw rate. A similar technique is employed in [li2015real] with
the addition of a penalization on final arc-length. In the context of local
trajectory planning, obstacles such as road boundaries, pedestrians, etc. are
not considered. Therefore, in this work, we focus on developing a velocity
profile, and bounds
$\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ that minimize a
cost function similar to [ziegler2014trajectory, ziegler2014making] that
penalizes arc-length, acceleration, jerk, and yaw rate.

##  II Problem Statement

This section begins with a review of the optimization problem that served as
motivation for the development of HCR/CCR curves in [banzhaf2018g]. This
optimization problem will then be augmented to account for comfort in Section
II-B. This augmented optimization problem is the focus of this paper. A note
on notation: for the remainder of this paper, $(\cdot)^{\prime}$ is used to
denote differentiation with respect to arc-length $s$ along a path, while
$\dot{(\cdot)}$ represents differentiation with respect to time.

###  II-A Original Optimization Problem

Let $\bm{p}=[x,y,\theta,\kappa,\sigma,v]^{T}$ denote a vector of states for a
vehicle. Here, $(x,y)\in\mathbb{R}^{2}$ is the planar location of the vehicle,
$\theta$ the heading, $\kappa$ the curvature, $\sigma$ the curvature rate, and
$v$ the velocity. In this paper, we refer to the states
$x,y,\theta,\kappa,\sigma$ as _path state_. We adopt the following common
vehicle model for the path states, assuming forward motion [banzhaf2018g]

$\begin{bmatrix}x^{\prime}\\\ y^{\prime}\\\ \theta^{\prime}\\\
\kappa^{\prime}\\\ \sigma^{\prime}\end{bmatrix}=\begin{bmatrix}\cos(\theta)\\\
\sin(\theta)\\\ \kappa\\\ \sigma\\\ 0\end{bmatrix}+\begin{bmatrix}0\\\ 0\\\
0\\\ 0\\\ \rho\end{bmatrix}.$ (1)

Here, derivatives are taken with respect to arc-length, $s$. Observe that the
model in (1) does not assume that $\rho$ is continuous. Thus equation (1) can
be used to describe $G3$ curves whose curvature rates are piece-wise linear
functions of arc-length. The goal of path planning using $G3$ curves, is to
obtain curves of minimal final arc-length, $s_{f}$, from start state
$(x_{s},y_{s},\theta_{s},\kappa_{s},\sigma_{s})$ to a goal state
$(x_{g},y_{g},\theta_{g},\kappa_{g},\sigma_{g})$, subject to boundary
constraints:

$\begin{split}x(0)&=x_{s},\ y(0)\ =y_{s},\ \ \theta(0)\ =\theta_{s},\
\kappa(0),\ =\kappa_{s},\\\ x(s_{f})&=x_{g},\ y(s_{f})=y_{g},\
\theta(s_{f})=\theta_{g},\ \kappa(s_{f})=\kappa_{g},\\\
\sigma(0)&=\sigma(s_{f})=0.\end{split}$ (2)

and physical constraints:

$\begin{split}\kappa(s)&\in[-\kappa_{\text{max}},\kappa_{\text{max}}],\
\forall s\in[0,s_{f}],\\\
\sigma(s)&\in[-\sigma_{\text{max}},\sigma_{\text{max}}],\ \forall
s\in[0,s_{f}],\\\ \rho(s)&\in[-\rho_{\text{max}},\rho_{\text{max}}],\ \forall
s\in[0,s_{f}].\end{split}$ (3)

To summarize, the goal of path planning using $G3$ curves with forward motion
as presented in [banzhaf2018g], is to solve the following optimal control
problem (OCP):

$\begin{split}\min_{\rho}\ &s_{f}\\\ s.t.\ \text{constraints}\
&\eqref{dynamics},\eqref{boundary},\eqref{bounds}\end{split}$ (4)

Problem (4) minimizes the final arc-length of a curve subject to the dynamic
model constraints, boundary constraints, and physical constraints in (1), (2),
and (43), respectively. Observe that at constant speed, minimizing arc-length
is equivalent to minimizing travel time.

###  II-B Adding Comfort Constraints

The question of how to determine the values of
$\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ is now addressed.
In [banzhaf2018g] and [1362698], the authors assume that these upper limits
are known. However, many possible bounds can exist. In order to minimize the
arc-length of a path as in (4), large values of
$\kappa_{\text{max}},\sigma_{\text{max}},\rho_{\text{max}}$ are desirable.
However, large boundary values may result in uncomfortable or even dangerous
trajectories, particularly at high speeds. It is for this reason that we
augment the OCP in (4) to penalize discomfort,

We therefore employ a technique similar to [levinson2011towards,
ziegler2014trajectory, ziegler2014making, li2015real], that penalizes a trade
off between travel time and comfort. In particular, let

$\begin{split}C=&\int_{0}^{t_{f}}(w_{a}C_{a}+w_{\mathcal{J}}C_{\mathcal{J}}+w_{y}C_{y}+w_{t})dt,\\\
\text{where}\ C_{a}&=|a_{N}(t)|^{2}+|a_{T}(t)|^{2}\\\
C_{\mathcal{J}}&=\mathcal{J}_{N}(t)^{2}+\mathcal{J}_{T}(t)^{2}=\left(\frac{d|a_{N}(t)|}{dt}\right)^{2}+\left(\frac{d|a_{T}(t)|}{dt}\right)^{2}\\\
C_{y}&=\left(\big{|}\frac{d\theta(t)}{dt}\big{|}\right)^{2}.\end{split}$ (5)

Here, $C_{a},C_{\mathcal{J}},C_{y}$ represent the squared acceleration, $a$
(broken into normal and tangential components $a_{N},a_{T}$), squared jerk
(broken into normal and tangential components
$\mathcal{J}_{N},\mathcal{J}_{T}$), and squared yaw rate. These costs are
weighted with appropriate weights $w_{a},w_{\mathcal{J}},w_{y}$. The weight
$w_{t}$ represents the relative importance of travel time.

As will be discussed later, we employ sequential quadratic programming (SQP)
techniques to optimize the velocity profile for a given path. In order to
employ these techniques, we require that the limits of integration in the
definition of the cost be fixed [biegler1984solution]. Noting that the final
arc-length of a given path is fixed, we re-parameterize the cost (5) in terms
of arc-length, $s$. Letting $a_{T}=a,da/dt=b$, we observe

$\begin{split}|a_{N}|&=v^{2}|\kappa|,\ |a_{T}|=|a|,\\\
\mathcal{J}_{N}&=\frac{d|a_{N}|}{dt}=2va|k|+1_{\kappa}v^{3}\sigma,\\\
\mathcal{J}_{T}&=1_{\alpha}b,\\\
\big{|}\frac{d\theta}{dt}\big{|}&=\big{|}\frac{d\theta}{ds}\frac{ds}{dt}\big{|}=|\kappa|v,\end{split}$
(6)

where $1_{x}$ represents the sign of the value $x$. Replacing (32) in (5), and
integrating with respect to $s$ instead of $t$, yields

$\begin{split}C&=\int_{0}^{s_{f}}w_{a}C_{a}^{\prime}+w_{\mathcal{J}}C_{\mathcal{J}}^{\prime}+w_{y}C_{y}^{\prime}+w_{t}C_{t}^{\prime}ds,\\\
C_{a}^{\prime}&=v^{3}|\kappa|^{2}+|a|^{2}v^{-1},\\\
C_{\mathcal{J}}^{\prime}&=v(2a|\kappa|+1_{\kappa}v^{2}\sigma)^{2}+|b|^{2}v^{-1},\\\
C_{y}^{\prime}&=|\kappa|^{2}v,\\\ C_{t}^{\prime}&=v^{-1}.\end{split}$ (7)

Because we have re-parameterized the cost in (5) in terms of arc-length, it is
convenient to note the following: if $\alpha=dv/ds,\beta=d\alpha/ds$, then

$a=\alpha v,\ b=v(\beta v+\alpha^{2}).$ (8)

We now use the cost function $C$ together with the constraints developed
earlier and the identity in (8), to state the new optimization problem that is
the focus of this paper:

$\begin{split}\min_{\rho,\beta}\ &C\\\ s.t.\ &\text{constraints
}\eqref{dynamics},\eqref{boundary},\eqref{alphabeta},\eqref{bounds}\\\
&\begin{bmatrix}v^{\prime}(s)&\alpha^{\prime}(s)\end{bmatrix}^{T}=\begin{bmatrix}\alpha(s)&\beta(s)\end{bmatrix}^{T}\\\
&v(0)=v_{s},\ v(s_{f})=v_{f},\ \alpha(0)=\alpha(s_{f})=0,\\\
&v(s)\in(0,v_{\text{max}}],a(s)\in[-a_{\text{max}},a_{\text{max}}],\\\
&b(s)\in[-b_{\text{max}},b_{\text{max}}],\ \forall s\in[0,s_{f}]\end{split}$
(9)

In the next section, we briefly describe the technique we employ to
approximate a solution to the optimization problem (9).

##  III Approach

In this section, we present the high-level idea behind out trajectory
generation algorithm before delving into the details of each step in the
following sections. At its highest level, the technique we employ is an
iterative process where each iteration involves two steps: Given a path
$P^{k}(s)$ with speed profile $v^{k}(s)$ at the start of iteration $k$,

Step 1:

compute a velocity profile $v^{k+1}(s)$ that minimizes the cost in (7)
assuming that $P^{k}(s)$ is the optimal path.

Step 2:

compute a new path $P^{k+1}(s)$ that minimizes (7) assuming that $v^{k+1}(s)$
is the optimal velocity profile.

The high-level technique described above guarantees a non-increasing cost
after each iteration. That is, if $C^{k}=C(P^{k},v^{k})$ represents the cost
(7) at the start of the $k^{th}$ iteration given $P^{k}(s),v^{k}(s)$, then
$C^{k}\geq C^{k+1}$. Indeed, $C^{k}=C(P^{k},v^{k})\geq C(P^{k},v^{k+1})$
because it is assumed that $v^{k+1}$ minimizes $C$ given $P^{k}$, and
$C(P^{k},v^{k+1})\geq C(P^{k+1},v^{k+1})=C^{k+1}$ because $P^{k+1}$ minimizes
$C$ given $v^{k+1}$.

Before describing our approach in more detail, present two motivating Lemmas:

######  Lemma III.1 (Criteria For Convexity).

The OPC in (9) is non-convex in general. However, it is convex if The
following Lemma is a direct consequence of …..

######  Lemma III.2 (Form of the Optimal Path).

If $(\rho^{*},\beta^{*})$ is a solution to the OCP (9), then $\rho^{*}$ is
bang-bang, exclusively taking values in $\\{\pm\rho_{\text{max}},0\\}$. That
is, the corresponding optimal path $P^{*}$ is a $G3$ path.

###### Proof.

The proof of Lemma (III.2) follows from the linearity of $\rho$ in the
Hamiltonian of the OCP (9), as well as Pontryagin’s minimum principle (see
[athans2013optimal]). ∎

Lemma III.2 implies that the augmentation of the OCP from (4) to (9) does not
change the fact that the optimal path is $G3$. This serves as motivation to
compute $G3$ paths during Step 2.

##  IV Computing $G3$ Paths

We begin this section with an investigation of single $G3$ curves. This
follows closely the work presented in [banzhaf2018g]. We then present an
algorithm to connect $G3$ curves to form $G3$ paths.

Figure 2: A basic $G3$ curve. Top: The functions $\sigma(s)$ (top),
$\kappa(s)$ (mid), and $\theta(s)$ (bottom). Bottom: The resulting curve in
the $x,y$ plane from start configuration $\bm{p}_{s}$ to goal configuration
$\bm{p}_{g}$.

###  IV-A Single $G3$ Curves

This section uses the definitions and notation presented in [banzhaf2018g] to
outline the general form of a $G3$ curve. An example $G3$ curve performing a
left-hand turn is presented in Figure 2. The curve begins at a path state
$\bm{p}_{s}=(x_{s},y_{s},\theta_{s},\kappa_{s},\sigma_{s}=0)$ at an arc-length
of $s=0$ along the path. From $s=0$ to $s=s_{1}$, the second derivative of
curvature, $\rho$, is set to its maximum value, $\rho_{\text{max}}$. Thus the
curvature rate $\sigma$ is given by the function
$\sigma(s)=\rho_{\text{max}}\cdot s$. The functions $\kappa(s),\theta(s)$ are
therefore quadratic and cubic (resp.), and can be calculated by (1).

At $s=s_{1}$, the curvature rate has reached its maximum allowable value
$\sigma_{\text{max}}$, and can go no higher, thus the function $\rho$ is set
to 0, and $\sigma(s)=\sigma_{\text{max}}$. As a result, for $s_{1}\leq s\leq
s_{2}$, the function $\kappa(s),\theta(s)$ are linear and quadratic (resp.).
At $s=s_{2}$, the value of $\rho$ is set to $-\rho_{\text{max}}$, and the
curvature rate decreases from $\sigma_{\text{max}}$, allowing the curvature to
reach a value $\kappa_{\text{top}}$ at $s=s_{3}$ (where
$|\kappa_{\text{top}}|\leq\kappa_{\text{max}}$). At $s=s_{3}$, the path state
is given by $\bm{p}(s_{3})$. From $s=s_{3}$ to $s=\Delta$, the curvature of
the curve is held constant at $\kappa_{\text{top}}$. Thus the curve remains at
a constant distance $\kappa_{\text{top}}^{-1}$ from its center of curvature
$\bm{x}_{c}$ for all $s_{3}\leq s\leq\Delta$. The circle $\Omega_{\text{I}}$
is the circle centered at $\bm{x}_{c}$ with radius $\kappa_{\text{top}}^{-1}$.

At $s=\Delta$, the value of $\rho$ is set to $-\rho_{\text{max}}$, and the
curvature rate follows the linear function
$\sigma(s)=-\rho_{\text{max}}(s-\Delta)$ until $s=s_{4}$ when
$\sigma(s)=-\sigma_{\text{max}}$. Upon reaching its minimum allowable value,
$\sigma(s)$ remains constant at $-\sigma_{\text{max}}$ for $s_{4}\leq s\leq
s_{5}$. At $s=s_{5}$, the value of $\rho$ is set to $\rho_{\text{max}}$,
allowing the curvature to descend to its final value $\kappa_{g}$ at
$s=s_{6}$. When $s=s_{6}$, the path state is given by $\bm{p}(s_{6})$ which
lies a distance of $r$ away from $\bm{x}_{c}$. The circle $\Omega_{\text{O}}$
is the circle centered at $\bm{x}_{c}$ of radius $r$.

To perform a right-hand turn, a similar analysis to that above is employed.
The resulting function $\sigma(s)$ will appear as a mirror images about the
horizontal axis to that presented in Figure 2, and the minimum-magnitude
curvature of the curve will be given by $-\kappa_{\text{max}}$.

We now illustrate how the switching arc-lengths $s_{i},i=1,\dots,6$ of the
curvature acceleration function $\rho$ may be calculated.

Given the parameters
$\rho_{\text{max}},\sigma_{\text{max}},\kappa_{\text{top}},\Delta$, the values
of $s_{i},\ i=1,\dots,6$ such that
$\kappa(0)=\kappa_{s},\kappa(s_{3})=\kappa_{\text{top}},\kappa(s_{6})=\kappa_{g}$,
are given by:

$\displaystyle s_{1}$
$\displaystyle=\begin{cases}\frac{\sigma_{\text{max}}}{\rho_{\text{max}}},\
&\text{if
}|\kappa_{\text{top}}-\kappa_{s}|>\frac{(\sigma_{\text{max}})^{2}}{\rho_{\text{max}}}\\\
\sqrt{\frac{|\kappa_{\text{top}}-\kappa_{s}|}{\rho_{\text{max}}}},\
&\text{otherwise}\end{cases}$ (10) $\displaystyle s_{2}$
$\displaystyle=\begin{cases}\frac{|\kappa_{\text{top}}-\kappa_{s}|}{\sigma_{\text{max}}},\
&\text{if
}|\kappa_{\text{top}}-\kappa_{s}|>\frac{(\sigma_{\text{max}})^{2}}{\rho_{\text{max}}}\\\
s_{1},\ &\text{otherwise}\end{cases}$ $\displaystyle s_{3}$
$\displaystyle=s_{1}+s_{2},$ $\displaystyle s_{4}$
$\displaystyle=\Delta+\begin{cases}\frac{\sigma_{\text{max}}}{\rho_{\text{max}}},\
&\text{if
}|\kappa_{\text{top}}-\kappa_{g}|>\frac{(\sigma_{\text{max}})^{2}}{\rho_{\text{max}}}\\\
\sqrt{\frac{|\kappa_{\text{top}}-\kappa_{g}|}{\rho_{\text{max}}}},\
&\text{otherwise}\end{cases}$ $\displaystyle s_{5}$
$\displaystyle=\Delta+\begin{cases}\frac{|\kappa_{\text{top}}-\kappa_{g}|}{\sigma_{\text{max}}},\
&\text{if
}|\kappa_{\text{top}}-\kappa_{g}|>\frac{(\sigma_{\text{max}})^{2}}{\rho_{\text{max}}}\\\
s_{4},\ &\text{otherwise}\end{cases}$ $\displaystyle s_{6}$
$\displaystyle=s_{4}+s_{5}-\Delta.$

These values are very similar to those presented in [banzhaf2018g], but are
extended to include variable initial and final curvatures. Also similar to
[banzhaf2018g], the curvature function of a $G3$ curve with curvature
acceleration switching times given by (10) with initial curvature
$\kappa_{s}$, is given by

$\kappa(s)=\begin{cases}\kappa_{s}\pm 0.5\rho_{\text{max}}s^{2},\
s\in[0,s_{1}]\\\ \kappa(\delta_{1})\pm\rho_{\text{max}}s_{1}(s-s_{1}),\
s\in[s_{1},s_{2}]\\\ \kappa(s_{2})\pm
0.5\rho_{\text{max}}(s-s_{2})(s_{2}+2s_{1}-s),\ s\in[s_{2},s_{3}]\\\
\kappa_{\text{top}},\ s\in[s_{3},\Delta]\\\ \kappa_{\text{top}}\mp
0.5\beta\rho_{\text{max}}(s-\Delta)^{2},\ s\in[\Delta,s_{4}]\\\
\kappa(s_{4})\mp\beta\rho_{\text{max}}(s_{4}-\Delta)(s-s_{4}),\
s\in[s_{4},s_{5}]\\\ \kappa(s_{5})\mp
0.5\beta\rho_{\text{max}}(s-s_{5})(s_{5}-2\Delta+2s_{4}-s).\end{cases}$ (11)

Here, the top sign of each ”$\pm,\mp$” is used if
$\kappa_{\text{top}}\geq\kappa_{s}$, while the bottom sign is used otherwise.
Furthermore, $\beta=1$ if $\sigma(s_{1})\sigma(s_{4})<0$ and $\beta=-1$
otherwise. The value $\beta=1$ is used in circumstances where the top
curvature $\kappa_{\text{top}}$ is the curvature of maximum magnitude on the
curve, i.e., when the curvature function increases to $\kappa_{\text{top}}$
and then decreases to $\kappa_{g}$, or decreases to $\kappa_{\text{top}}$ and
then increases to $\kappa_{g}$. On the other hand, a value $\beta=-1$ is used
when the curvature function is either monotonically increasing or decreasing
over the entire curve.

By (1), the path state vector, $\bm{p}$ along a $G3$ curve whose curvature is
given by (11), can be parameterized in terms of arc-length as:

$\bm{p}(s)=\begin{bmatrix}x(s)\\\ y(s)\\\ \theta(s)\\\
\kappa(s)\end{bmatrix}=\begin{bmatrix}x_{s}+\int_{0}^{s}\cos(\theta(\tau))d\tau\\\
y_{s}+\int_{0}^{s}\sin(\theta(\tau))d\tau\\\
a_{0}+a_{1}s+a_{2}s^{2}+a_{3}s^{3}\\\ \kappa(s)\end{bmatrix},$ (12)

where $a_{0},a_{1},a_{2},a_{3}$ are obtained by integrating $\kappa(s)$ in
(11). As Figure 2 implies, the point $\bm{x}_{c}$, and radius $r$, are given
by

$\bm{x}_{c}=\begin{bmatrix}x_{c}\\\
y_{c}\end{bmatrix}=\begin{bmatrix}x(s_{3})-\kappa_{\text{top}}^{-1}\sin(\theta(s_{3}))\\\
y(s_{3})+\kappa_{\text{top}}^{-1}\cos(\theta(s_{3}))\end{bmatrix},$ (13)
$r=||(x(s_{6}),y(s_{6}))-(x_{c},y_{c})||.$ (14)

Assuming that $\rho_{\text{max}},\sigma_{\text{max}}$ are known, we observe
from the preceding arguments, that the initial path states $\bm{p}_{s}$, the
top and final curvatures $\kappa_{\text{top}},\kappa_{g}$ (resp.), and the
arc-length $\Delta$ are enough to uniquely define a $G3$ curve. Thus we denote
a $G3$ curve $P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$ as the set
of path states $\bm{p}(s)$ given by (12) from $s=0$ to $s=s_{6}$. As (10)
implies, the final arc-length of this curve is given by $s_{6}$. Let
$P_{i}^{s_{j}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$, be the
value of path state $i$ at arc-length $s_{j}$.

In the next section, we investigate a set of curves of the form
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta)$ with zero final
curvature where $\Delta$ is the only parameter allowed to vary. We will show
that given such a set, we may construct a circle whose tangent lines become
coincident with every member of the set. This result will be heavily leveraged
in the final $G3$ curve construction algorithm.

###  IV-B Sets of Single $G3$ Curves and Representative Circles

This section assumes that $\rho_{\text{max}},\sigma_{\text{max}}$ are known,
and proposes a reduction of the $G3$ curve path planning problem to a Dubin’s
path planning problem . For fixed values
$\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\sigma_{\text{max}},\rho_{\text{max}}$,
consider the set of curves

$\mathcal{P}=\\{P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)\ :\
\Delta\geq s_{3},\ \kappa_{g}=0\\}.$ (15)

The above set is defined for curves whose final curvature is 0. The reason for
this, is that we will eventually be connecting $G3$ curves with other $G3$
curves using straight lines. Therefore, we are interested in investigating
curves that can be connected to straight lines while preserving continuous
curvature. The assumption that $\sigma(s_{6})=0$ is the final curvature rate
of each curve ensures that $G3$ curves can be connected with straight lines
while preserving continuous curvature rate.

Note that every set $\mathcal{P}$ described in (15) has the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})$ as an element.
This is a curve that spends no time at constant curvature (see Figure 2). Upon
reaching curvature $\kappa_{\text{top}}$, the curvature of this curve
immediately increases or decreases to $\kappa_{g}$.

Observe that for fixed values $\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0$,
the value of $s_{3}$ given in (10) is independent of $\Delta$, implying that
every curve in $\mathcal{P}$ shares the same switching arc-length $s_{3}$. The
same holds for the center $\bm{x}_{c}$ given in (13). Therefore, the values
$\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0$, which are shared by all curves
in $\mathcal{P}$, are sufficient to determine $s_{3}$ and $\bm{x}_{c}$.
Finally, note that each curve in $\mathcal{P}$ is coincident with every other
curve in $\mathcal{P}$ for all $s\leq s_{3}$. This phenomena can be see in
Figure 2: the duration, $\Delta-s_{3}$ that a curve spends at constant
curvature $\kappa_{\text{top}}$ does not affect the portion of the curve
between $\bm{p}_{s}$ and $\bm{p}(s_{3})$.

The following Lemma illustrates the relationship between $\Delta$ and
$P^{s_{6}}_{\theta}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$. In
particular, it shows that for any fixed values of
$\kappa_{\text{top}},\sigma_{\text{max}},\rho_{\text{max}}$, a value of
$\Delta$ can be computed to ensure that
$P^{s_{6}}_{\theta}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)=\theta_{g}$
for any desired final heading $\theta_{g}$. Moreover, the required value of
$\Delta$ varies linearly with $\theta_{g}$.

######  Lemma IV.1 (Final Heading).

For each set of curves $\mathcal{P}$ defined in 15, there is a unique value,
$\Delta$, to the equation
$P^{s_{6}}_{\theta}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)=\theta_{g}$
such that $P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$ has minimum
arc-length:

$\Delta=s_{3}+(\theta_{g}-P_{\theta}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta=s_{3}))\kappa_{\text{top}}^{-1}.$
(16)

where $s_{3}$ is given by (10) for the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$.

The proof of Lemma IV.1 can be found in the Appendix. Lemma IV.1 implies that
if $\kappa_{\text{top}},\sigma_{\text{max}},\rho_{\text{max}}$ are fixed, then
the final heading of a curve,
$C_{\theta}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$, varies
linearly with $\Delta$. This Lemma motivates the following key Theorem that
will be heavily leveraged later. The result of the Theorem is demonstrated in
Figure 3 (Mid).

Figure 3: (Left) Example representative circle, $\Omega_{D}$, including point
$(x_{D},y_{D})$. (Mid) Illustration of Theorem IV.2. (Right) Illustration of
Corollary IV.4, including partial Dubin’s path (blue) and corresponding $G3$
curve.

######  Theorem IV.2 (Reducing Theorem).

For a given set of curves $\mathcal{P}$ as defined in (15), Let
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$ be any curve in
$\mathcal{P}$. Then the shortest distance from the point $\bm{x}_{c}$ defined
in (13) to the line passing through
$\big{(}P_{x}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta),P_{y}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)\big{)}$
whose slope is
$\tan\big{(}P_{\theta}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)\big{)}$
is a constant function of $\Delta$.

The proof of Theorem IV.2 can be found in the Appendix. The result of Theorem
IV.2 is shown in Figure 3 (Mid). In this figure, the red dots correspond to
the points on the lines containing the final path states of each curve in
$\mathcal{P}$ that are closest to $\bm{x}_{c}$. Observe that they lie on a
circle centered at $\bm{x}_{c}$. This theorem has the following major
consequence that allows us to reduce a $G3$ path planning problem to a Dubin’s
path planning problem if
$\bm{p}_{s},\bm{p}_{g},\kappa_{\text{top}},\sigma_{\text{max}},\rho_{\text{max}}$
are fixed, and only $\Delta$ may vary. This interim result is the foundation
of our proposed $G3$ path planning algorithm. We begin with a definition.

######  Definition IV.3 (Representative Circle).

Given a set of curves $\mathcal{P}$ defined above, let
$x_{f},y_{f},\theta_{f}$ denote the final $x,y$, and $\theta$ values of the
curve $P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})$, and let
$m$ denote the slope of a line induced by $\theta_{f}$, i.e.,
$m=\tan{\theta_{f}}$. The _representative circle_ , denoted $\Omega_{D}$ is
the circle centered at $\bm{x_{c}}=[x_{c},y_{c}]^{T}$ containing the point
$(x_{D},y_{D})$ where

$\begin{split}x_{D}=&\begin{cases}\frac{(mx_{f}-y_{f}+m^{-1}x_{c}+y_{c})}{m+m^{-1}},\
&\text{if }\theta_{f}\neq\pi/2\\\ x_{f},\ &\text{otherwise}\end{cases}\\\
y_{D}=&\begin{cases}-m^{-1}(x_{D}-x_{c})+y_{c},\ &\text{if
}\theta_{f}\neq\pi/2\\\ y_{c}\ &\text{otherwise}.\end{cases}\end{split}$ (17)

That is, for the straight line containing $(x_{f},y_{f})$ whose slope is
$\tan\theta_{f}$, the representative circle is the circle centered at
$\bm{x}_{c}$ containing the point on the line closest to $\bm{x}_{c}$. An
example representative circle, and corresponding points $(x_{D},y_{D})$ is
show in Figure 3 (Left). In this figure, the black curve is
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})$.

The following Corollary uses the preceding Lemma and Theorem to reduce the
$G3$ path planning problem to a Dubin’s path planning problem [Dubins_1957]
provided that the start and goal path states are sufficiently far apart.
Dubin’s path planning problems are easily solved, and for this reason the
following corollary motivates the algorithm for computing $G3$ paths presented
in the next section.

######  Corollary IV.4 (Dubin’s Reduction).

Consider a set of curves $\mathcal{P}$ defined in (15) and let $\Omega_{D}$
represent the representative circle for the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})\in\mathcal{P}$.
Let $\mathcal{D}$ denote a Dubin’s path originating at
$(x_{D},y_{D})\in\Omega_{D}$ with orientation corresponding to
$\kappa_{\text{top}}$. If $\mathcal{D}$ is comprised of two curves connected
with a straight line (which exists provided start and goal states are
sufficiently far apart), then there exists a unique curve in $\mathcal{P}$
whose final path states are coincident with a point on the straight portion of
$\mathcal{D}$. Moreover, if $\theta_{g}$ represents the angle inscribed by the
straight portion of $\mathcal{D}$, then the curve in $\mathcal{P}$ coincident
with $\mathcal{D}$ is characterized by $\Delta$ given in (16).

The proof of Corollary IV.4 follows directly from Lemma IV.1, and Theorem
IV.2. This result is illustrated in Figure 3 (Right). Here the blue path
corresponds to a Dubin’s path $\mathcal{D}$ originating from $(x_{D},y_{D})$
on the representative circle $\Omega_{D}$, whose orientation corresponds to
$\kappa_{\text{top}}$. The final heading after the first curve in
$\mathcal{D}$ can be calculated analytically from the arc-length of this first
path segment (from red dot to red dot). Using (16), a value of $\Delta$ is
computed which fixes the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta)$ (black curve).

Observe now that if $\kappa_{\text{top}}$ is known, then the $G3$ path
planning problem can be reduced to a Dubin’s path planning problem (provided
start and goal path states are sufficiently far apart) using Corollary IV.4.

An example of the technique is illustrated in Figure 4. In the left image, we
see the curves $P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})$,
and $P(\bm{p}^{(2)},-\kappa_{\text{top}},\kappa_{g}=0,\Delta=s_{3})$ where
$\bm{p}^{(2)}$ is the reverse orientation of the goal state vector
$\bm{p}_{g}$. We also see the representative circles for each curve as well as
the points $(x_{D},y_{D})$ on each circle. In the middle image, we see the
Dubin’s solution between start and goal points corresponding to the points
$(x_{D},y_{D})$ on each representative circle. The Dubin’s solution provides
an angle for the straight portion of its path. Finally, in the image on the
right, we use Corollary IV.4 to determine two values of $\Delta$ analytically
from (16) given the desired Dubin’s angle. These two values of $\Delta$ fix
two curves that can be connected via a straight line (black path).

The observation that $G3$ curve path planning problems can be reduced to
Dubin’s path planning problems if $\kappa_{\text{top}}$ is known, is the
foundation of the $G3$ curve generating algorithm presented in the next
section.

Figure 4: (Left) Representative circles with points originating from start
states, and the reverse of the goal state. (Mid) Dubin’s solution between
start and goal states. (Right) Solution path using Corollary IV.4. Figure 5:
(a) Continua of curves with initial configurations $\bm{p}^{(1)}$,
$\bm{p}^{(2)}$. (b) trajectory with $C_{1}\in S^{+},C_{2}\in G^{-}$.

###  IV-C Main Algorithm

Let $\bm{p}^{(2)}=[x_{g},y_{g},\theta_{g}+\pi,-\kappa_{g}]$ denote the goal
state vector $\bm{p}_{g}$ oriented in reverse, and consider the sets

$\begin{split}S^{+}&=\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)},\kappa_{g}=0,\Delta^{(1)}):0\leq\kappa_{\text{top}}^{(1)}\leq\kappa_{\text{max}}\\}\\\
S^{-}&=\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)},\kappa_{g}=0,\Delta^{(1)}):-\kappa_{\text{max}}\leq\kappa_{\text{top}}^{(1)}<0\\}\\\
G^{+}&=\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)},\kappa_{g}=0,\Delta^{(2)}):0\leq\kappa_{\text{top}}^{(2)}\leq
k_{\max}\\}\\\
G^{-}&=\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)},\kappa_{g}=0,\Delta^{(2)}):-\kappa_{\text{max}}\leq\kappa_{\text{top}}^{(2)}<0\\}.\end{split}$
(18)

These sets, illustrated in Figure 5 (a), form four continua of curves
originating from $\bm{p}^{(1)}$ and $\bm{p}^{(2)}$. As
$\sigma_{\text{max}},\rho_{\text{max}}\rightarrow\infty$, the continua
$S^{+},S^{-},G^{+},G^{-}$ approach the four circles incident on
$\bm{p}_{s},\bm{p}_{g}$ that are used during the computation of Dubins’ paths.

The high-level idea behind the algorithm presented here, is as follows: for
each continuum in $\\{S^{+},S^{-}\\}$, say, $S^{\pm}$, and each continuum in
$\\{G^{+},G^{-}\\}$, say $G^{\pm}$, determine curves $P1\in S^{\pm}$, $P2\in
G^{\pm}$ that can be connected with either a straight line, or another curve.
Moreover, select $P1,P2$ in such a way that the resulting path has shortest
path length. Finally, select the shortest of the four resulting paths. To this
end, we assume that the curves $Pi,i=1,2$ attain their maximum top curvatures:
$\kappa_{\text{top}}^{(i)}=\pm\kappa_{\text{max}}$. Thus $Pi$ is uniquely
defined by its value of $\Delta^{(i)}$. We then use the technique of the
previous section to reduce the problem of connecting $P1,P2$ to a Dubin’s path
planning problem. Finally, we relax the assumption that each curve reaches its
maximum curvature, and perform corrections. This idea is formalized here.

For each continuum in $\\{S^{+},S^{-}\\}$, say $S^{\pm}$, and each continuum
in $\\{G^{+},G^{-}\\}$, say $G^{\pm}$, perform the following:

Figure 6: Illustration of Algorithm.

Step 1. Initial Assumption:

Begin by defining two curves $Pi,i=1,2$ originating from $\bm{p}^{(i)}$ whose
top curvature is maximal, and whose value of $\Delta^{(i)}$ is $s_{3}^{(i)}$
where $s_{3}^{(i)}$ is given by (10) for the curve $Pi$:

$\begin{split}Pi=P(\bm{p}^{(i)},\kappa_{\text{top}}^{(i)}=\pm\kappa_{\text{max}},\kappa_{g}=0,\Delta^{(i)}=s_{3}^{(i)}).\end{split}$

The sign of $\kappa_{\text{top}}^{(i)}$ is chosen in accordance with the
selected continuum. Step 1 is illustrated in Figure 6.

Step 2. Compute Representative Circles:

We now compute the centers $\bm{x}_{c}^{(i)},i=1,2$ (given by (13)), points
$(x_{D}^{(i)},y_{D}^{(i)})$ (given by (17)), and radii of the representative
circles $\Omega_{D}^{(i)}$ of curves $Pi$. The radius $r_{D}^{(i)}$ of the
representative circles is given by
$r_{D}^{(i)}=||\bm{x}_{c}^{(i)}-(x_{D}^{(i)},y_{D}^{(i)})||$. Because curves
$Pi$ have the same magnitude of top curvature and same final curvature
$\kappa_{g}^{(i)}=0$, it follows that $r_{D}^{(1)}=r_{D}^{(2)}$. We let
$r_{D}=r_{D}^{(1)}$. Step 2 is illustrated in Figure 6.

Step 3. Solve Dubin’s Problem:

We now solve the Dubin’s path planning problem from start state to goal state,
where

$\begin{split}\text{start}=\big{(}x_{D}^{(1)},y_{D}^{(1)},\theta^{(1)}(s_{6}^{(1)})\big{)},\
\text{goal}=\big{(}x_{D}^{(2)},y_{D}^{(2)},\theta^{(2)}(s_{6})+\pi\big{)},\end{split}$

with a minimum turning radius of $r_{D}$. In particular, we search for a
Dubin’s solution of the type C±SC± where ”$+$” denotes a left-hand (positive
curvature) turn and ”$-$” a right (negative curvature). Signs are chosen to
match the signs of the chosen continua. That is,

$\begin{split}\kappa_{\text{top}}^{(1)}=\kappa_{\text{max}}\Rightarrow
C^{+}SC,\ \kappa_{\text{top}}^{(1)}=-\kappa_{\text{max}}\Rightarrow
C^{-}SC,\\\ \kappa_{\text{top}}^{(2)}=\kappa_{\text{max}}\Rightarrow CSC^{-},\
\kappa_{\text{top}}^{(2)}=-\kappa_{\text{max}}\Rightarrow CSC^{+}\end{split}$

The sign of the second curve is opposite to the second top curvature, as the
second curve is traversed in reverse. An example of the Dubin’s problem and
solution appear as the yellow curve in Figure 6 (Steps 3,4).

Step 4. Calculate Exiting Angle:

The Dubin’s solution provides an _exiting angle_ , $\theta_{\text{exit}}$
which is the angle formed by the straight portion of the Dubin’s solution. In
particular, $\theta_{\text{exit}}=s_{f}r_{D}^{-1}+\theta_{s}$ where $s_{f}$ is
the arc-length of the first curve in the Dubin’s solution. The exiting angle
is illustrated in Figure 6 (Steps 3,4).

Step 5. Relax Maximum Curvature Assumption:

While we can find a curves whose top curvatures are $\pm\kappa_{\text{max}}$,
and whose final headings are $\theta_{\text{exit}}$, we observe that they may
not have the shortest path-length. We therefore seek to relax the assumption
that the top curvature of each curve reaches its maximum value. To this end,
for each curve $Pi$, we compute the shortest curve whose final heading is
$\theta_{\text{exit}}+\pi(i-1)$ (see Figure 6 (Step 5.)). Observe that by
integrating the curvature function in (11) from $s=0$ to $s=s_{6}$, we obtain
an expression for the final heading of a curve as a function of
$\kappa_{\text{top}}$. For each curve $Pi$, we set $\Delta^{(i)}=s_{3}^{(i)}$
in this expression, and equate the result to $\theta_{\text{exit}}+\pi(i-1)$
(modulo $2\pi$), resulting in an equation of one variable
$\kappa_{\text{top}}^{(i)}$ that can be solved numerically. The result is the
smallest magnitude $\kappa_{\text{top}}^{(i)}$ such that the final heading of
the curve $Pi$ is $\theta_{\text{exit}}+\pi(i-1)$. If the solution
$|\kappa_{\text{top}}^{(i)}|>\kappa_{\text{max}}$, then we set
$|\kappa_{\text{top}}^{(i)}|=\kappa_{\text{max}}$, and compute the required
value of $\Delta^{(i)}$ that results in the desired final heading using (16).

Step 6. Correct Relaxation Error:

We redefine the two curves $P1,P2$ as

$Pi=(\bm{p}^{(i)},\kappa_{\text{top}}^{(i)},\kappa_{g}=0,\Delta^{(i)}),\
i=1,2,$

where $\kappa_{\text{top}}^{(i)},\Delta^{(i)}$ were computed in Step 5. Let
$(x_{f}^{(i)},y_{f}^{(i)},\theta_{f}^{(i)})$ denote the final path
configurations of path $Pi$. Observe that
$\theta_{f}^{(i)}=\theta_{\text{exit}}+\pi(i-1)$. Observe further, that in
order to connect $P1$ with $P2$ via a straight line, we require that

$\theta_{\text{exit}}=\text{atan}_{2}\left(y_{f}^{(2)}-y_{f}^{(1)},x_{f}^{(2)}-x_{f}^{(1)}\right).$
(19)

That is, the line connecting the final $(x,y)$-positions of each curve must
have slope matching the curve’s final heading. We define the connection angle,
$\theta_{\mathrm{connect}}$ as the right hand side of the above equation, and
the connection error, $\theta_{\mathrm{error}}$ as
$\theta_{\mathrm{connect}}-\theta_{\mathrm{exit}}$. The exiting and connection
angle are illustrated in Figure 6. If $\theta_{\mathrm{error}}$ is within a
pre-specified tolerance $\epsilon$, the algorithm terminates. Otherwise, we
seek to make $|\sin(\theta_{\mathrm{error}})|\leq\sin(\epsilon)$ for reasons
that will become evident. This is done by recursively assigning
$\theta_{\mathrm{exit}}=\theta_{\mathrm{connect}}$, and repeating Steps 5, 6
until either $\sin(\theta_{\mathrm{error}})$ lies within the desired
tolerance, or it switches sign. If $\sin(\theta_{\mathrm{error}})$ switches
sign, perform a binary search on $\theta_{\mathrm{exit}}$ until the error
meets the tolerance. We call $P1$ the resulting curve that originates from
$\bm{p}^{(1)}$, and $P2$ the curve that originates from $\bm{p}^{(2)}$ as seen
in Figure 6. If $\theta_{\mathrm{error}}\approx 0$, the algorithm terminates
here, returning $P1,P2$, and the line connecting these curves.

Figure 7: Step 7. (Left) Illustration of overlap. start and goal
configurations too close together resulting in curve $P2$ terminating behind
$P1$. (Right) Stretching of curves $P1,P2$ by decreasing $\sigma_{\text{max}}$
until $P1,P2$ can be connected by third curve.

######  Theorem IV.5 (Shortest Curve).

Let $S^{\pm},G^{\pm}$ be the continua chosen at the beginning of Step 1. If
the algorithm terminates at the end of Step 6, with
$\theta_{\mathrm{error}}=0$, then the resulting paths $P1,P2$ and the straight
line connecting them is the shortest path connecting start and goal path
states that connect the continua $S^{\pm},G^{\pm}$.

There are two situations that might cause the above algorithm to fail to
produce a path. The first case arises from the observation that in Step 6, we
minimize the sine of the connection error yielding
$\sin(\theta_{\text{error}})\approx 0$. Thus there are two possibilities:
either $\theta_{\text{error}}\approx 0$ implying that
$\theta_{\text{exit}}\approx\theta_{\text{connect}}$, in which case the curves
$P1,P2$ can be connected with a straight line as desired, or
$\theta_{\text{exit}}-\theta_{\text{connect}}\approx\pi$, implying that the
curves $P1,P2$ _overlap_ , that is, that the straight line connecting the
final states of $P1,P2$ must be traversed backwards. This phenomenon, which is
illustrated in Figure 7 (Left), can only occur when
$||\bm{x}_{c}^{(1)}-\bm{x}_{c}^{(1)}||<r^{(1)}+r^{(2)}$ where $r^{(i)}$ is
given in (14) for the curve $P^{(i)}$ obtained at the end of the algorithm. To
compensate for this overlap, we observe that if $P1,P2$ are chosen from
continua of opposite sign (that is, $P1,P2$ are two right or two left turns),
then overlap may occur because there is not sufficient room between start and
goal states for $P1,P2$ to reach 0 final curvature and still be connectable.
However, because the two curves have top curvatures
$\kappa_{\text{top}}^{(1)},\kappa_{\text{top}}^{(2)}$ of the same sign there
is no need for the two curves to attain zero final curvature to be connected
smoothly. Therefore, we attempt to connect $P1,P2$ with a third curve whose
top curvature $\kappa_{\text{top}}^{(3)}$ has the same sign as the top
curvatures of the other curves. We offer the following supplemental step

Step 7. Correct Overlap: If $\theta_{\text{error}}\approx\pi$ and
$\kappa_{\text{top}}^{(1)}\kappa_{\text{top}}^{(2)}\leq 0$ (implying that $P2$
traversed in reverse orientation has a top curvature of the same sign as that
of $P1$), declare the new start and goal states as
$\bm{p}_{s}^{\prime}=P1(\Delta^{(1)})$, $\bm{p}_{g}^{\prime}$ is
$P2(\Delta^{(2)})$ with reverse orientation (negative of the curvature, and
add $pi$ to the heading). Define the curve

$P3=(\bm{p}_{s}^{\prime},\kappa_{\text{top}}^{(3)},\kappa_{g}=-\kappa^{(2)}(\Delta^{(2)}),\Delta^{(3)}).$

Observe that we require that the final heading of this curve by
$\theta_{g}^{\prime}=\theta^{(2)}(\Delta^{(2)})$. Therefore, for any
$\kappa_{\text{top}}^{(3)}$, we can determine an appropriate value of
$\Delta^{(3)}$ satisfying this new heading requirement, using (16). Thus there
is only one variable that we require: $\kappa_{\text{top}}^{(3)}$. We minimize
the distance between the final $(x,y)$-position of $P3$, and
$(x^{(2)}(\Delta^{(2)}),y^{(2)}(\Delta^{(2)}))$ over
$\kappa_{\text{top}}^{(3)}\in[-\kappa_{\text{max}},\kappa_{\text{max}}]$ such
that $\kappa_{\text{top}}^{(3)}$ has the same sign as
$\kappa_{\text{top}}^{(1)}$. If the resulting error is within acceptable
limits, we terminate the algorithm, connecting $P1,P2$ with $P3$. Otherwise,
we slowly decrement the value of $\sigma_{\text{max}}$ and go back to step 1
until a pre-specified lower limit of $\sigma_{\text{max}}$ has been reached.
By decrementing $\sigma_{\text{max}}$, we ”open up” the curves $P1,P2$. This
phenomena is best described pictorially, and can be seen in Figure 7 (Right).
If $\sigma_{\text{max}}$ has reached its lower limit, and no solution has been
found, we move on to Step 8 discussed next.

The second situation in which the algorithm may fail, occurs in Step 3, in
which a Dubin’s solution that matches chosen continua type is determined. If
it is desired that the two curve segments of the Dubin’s solution have the
same sign, then there will always exist a Dubin’s solution of type CSC as any
two circles of equal radii will always have at least two outer tangents.
However, if it is desired that the two curves in the Dubin’s solution have
opposite sign, then a solution to the Dubin’s path problem will only have a
solution if $||\bm{x}_{c}^{(1)}-\bm{x}_{c}^{(1)}||\geq 2r_{D}$. This is
illustrated in Figure 8 (Left).

To compensate for the second situation in which the algorithm may fail, that
is, when the Dubin’s problem in Step 3 fails to have a solution, we implement
the following ”run away” technique. This technique can be seen as a ”catch
all” technique that will always produce a (possibly sub-optimal) solution if
any previous step fails. The high level idea is to select a path originating
at $\bm{p}^{(1)}$ whose final states can be connected to $\bm{p}^{(2)}$ using
the techniques outlined in Steps 1-7. That is, if start and goal states are
too close together, we begin by running away from the start state until we
reach a new state that is sufficiently far from the goal state. This technique
is motivated by Dubin’s solutions of type CCC. In particular, if start and
goal states are too close together to admit a solution of the form
$C^{\pm}SC^{\mp}$, then the optimal path is to first ”run away” from the start
state along a circle $C^{\mp}$, until a point is reached where a solution of
the form $C^{\pm}SC^{\mp}$ exists (here $S$ is a single point). This results
in a solution $C^{\mp}C^{\pm}SC^{\mp}$ where $S$ is a single point.

Step 8. Run Away: If we have not yet reached a solution, we call the following
catch-all algorithm to ensures that a curve is produced:

Algorithm 1 Run Away Algorithm

1:procedure RunAway($\bm{p}_{s},\bm{p}_{g}$)

2: $\kappa_{\text{top}}^{(0)}=0$

3: while Steps 1-6 fail to produce curve do

4:
$P0=P(\bm{p}^{(0)},\kappa_{\text{top}}^{(0)},\kappa_{g}=0,\Delta^{(0)}=s_{3}^{(0)})$

5: $x_{f},y_{f},\theta_{f},\kappa_{f}=P0(s_{6}^{(0)})$

6: $\theta_{\text{between}}=$ angle of line connecting
$\bm{x}_{c}^{(1)},\bm{x}_{c}^{(2)}$

7: if $|\theta_{between}-\theta_{f}|\leq\pi/4$ then

8: $\Delta^{(0)}$ chosen to make $P0$ shortest curve with final heading equal
to $\theta_{\text{between}}$ (see equation (16)).

9: $\kappa_{\text{top}}^{(0)}=\kappa_{\text{top}}^{(0)}-\epsilon\
\text{sign}(\kappa_{\text{top}}^{(1)})$

10: $\bm{p}_{s}=P0(s_{6}^{(0)})$

11: Go to Step 1 of main algorithm

The result of the RunAway algorithm can be seen in Figure 8 (Right). The
algorithm constructs a curve $P0$ originating at $\bm{p}_{s}$. In step 6, we
define the angle $\theta_{\text{between}}$ as the angle connecting the centers
of the representative circles of $P1,P2$. In order to return a solution, it is
desirable that these centers be far apart, as we have already discussed.
Therefore, the RunAway algorithm computes a curve $P0$ that travels in the
direction of the line connecting $\bm{x}_{c}^{(1)},\bm{x}_{c}^{(2)}$. At each
step of the while loop, a new start state is computed as the end state of
curve $P0$, and steps 1-7 are re-attempted from the new start state. This
recursive process, including the representative circles of the curves $P1,P2$
at each iteration of the while loop are shown in Figure 8 (Right). Observe
that the blue curve is attempting to drive the centers of the representative
circles farther apart.

Figure 8: Step 8. (Left) No Dubins’ solution of type $C^{+}SC^{-}$ is
possible. Step 3 Fails. (Right) Illustration of ”run away” technique. Blue
curve (P0) is run away portion.

##  V Computing Velocity Profiles

In this section we describe how, given a $G3$ path, to compute a velocity
profile $v(s)$ that minimizes the cost function in (5) over the path. At a
high level, the process can best be described as an instance of Sequential
Quadratic Programming (SQP). To wit, given a $G3$ curve $C$ with a final arc-
length of $s_{f}$, we begin by sampling $n\in\mathbb{N}_{\geq 3}$ arc-lengths
$\\{s_{1}=0,s_{2},s_{3},\dots,s_{n}=s_{f}\\}$ the interval $[0,s_{f}]$, and we
assume that the optimal velocity, $v^{*}(s)$ that minimizes (5) can be
approximated with a polynomial of arc-length $s$ with degree no more than
$n+1$ between $s=0$ and $s=s_{f}$. This assumption allows us to make the
following simplification:

$\alpha^{*}(s)\approx\hat{\alpha}=\sum_{i=1}^{n}p_{i}s^{i},\
v^{*}(s)\approx\hat{v}=v_{s}+\sum_{i=1}^{n}\frac{p_{i}}{i+1}s^{i+1}.$ (20)

The values of $p_{i}$ starting at $i=1$ instead of $i=0$, arises from the fact
that $\alpha^{*}(0)=0$ by assumption. Thus, to compute the polynomial
$\hat{v}(s)$, it suffices to compute the $n$ decision variables summarized as
a vector $\bm{p}=[p_{1},\dots,p_{n}]$. Determining $\bm{p}$ will be done using
SQP, which is a process that iterates over guesses of values of $\bm{p}$.

Given tuples $(s_{i},a_{i}),i=2,\dots,n$ with $a_{n}=0$, we begin by finding
the polynomial $\hat{\alpha}(s)$ of degree $n$ such that
$\hat{\alpha}(s_{i})=a_{i}$, and $\hat{v}(s_{f})=v_{f}$. Observe that
$a_{n}=0$ is to reflect that boundary condition $\alpha^{*}(s_{f})=0$. This is
done by solving a system of $n$ linear equations, the solution to which may be
expressed as a vector $\bm{p}(\bm{a})$ given by

$\bm{p}(\bm{a})=\begin{bmatrix}p_{1}\\\ p_{2}\\\ \vdots\\\
p_{n}\end{bmatrix}=V^{-1}\begin{bmatrix}v_{f}-v_{s}\\\ a_{2}\\\ \vdots\\\
a_{n}=0\end{bmatrix}$ (21)

where

$V=\begin{bmatrix}s_{f}^{2}/2&s_{f}^{3}/3&s_{f}^{4}/4&\dots&s_{f}^{n+1}/(n+1)\\\
s_{2}&s_{2}^{2}&s_{2}^{3}&\dots&s_{2}^{n}\\\
s_{3}&s_{3}^{2}&s_{3}^{3}&\dots&s_{3}^{n}\\\
\vdots&\vdots&\vdots&\ddots&\vdots\\\
s_{n}&s_{n}^{2}&s_{n}^{3}&\dots&s_{n}^{n}\end{bmatrix}.$ (22)

The matrix $V$ in (22) is always non-singular provided $s_{i}\neq s_{j}\forall
i\neq j,i,j\in\\{1,\dots,n\\}$. Moreover, if the sampling arc-lengths $s_{i}$
are fixed, then $V$ is independent of the choice of way-points $a_{i}$,
implying that $V^{-1}$ need only be computed once for each proposed set
$\\{a_{2},\dots,a_{n}\\}$.

For fixed curvature, curvature rate, and curvature acceleration profiles
$\kappa(s),\sigma(s),\rho(s)$ for all $s\in[0,s_{f}]$, let

$\begin{split}\mathcal{L}\big{(}v,\alpha,\beta\big{)}=&w_{a}C_{a}+w_{\mathcal{J}}C_{\mathcal{J}}+w_{y}C_{y}+w_{t}C_{t}ds\\\
&+\delta(\alpha,\alpha_{\text{max}})+\delta(v,v_{\text{max}})+\delta(\beta,\beta_{\text{max}})\end{split}$
(23)

where $\delta(x,x_{\text{max}})=(\max(|x|-x_{\text{max}},0))^{M}$ and $M$ is
large represents a boundary function penalizing values of $|x|$ above
$x_{\text{max}}$. where $C_{a},C_{y},C_{\mathcal{J}},C_{t}$ are given in (33).
Let us fix the values of $s_{i}$, and define a set of non-fixed way-points
$\bm{a}=[a_{2},\dots,a_{n-1}]$. The cost of such a choice of way-points is
therefore given by

$\begin{split}C(\bm{a})&=\int_{0}^{s_{f}}\mathcal{L}(v(s,\bm{a}),\alpha(s,\bm{a}),\beta(s,\bm{a}))ds\\\
v(s,\bm{a})&=v_{s}+\bm{p}(\bm{a})^{T}\left[\frac{s^{i+1}}{i+1},i=1\dots,n\right]^{T}\\\
\alpha(s,\bm{a})&=\bm{p}(\bm{a})^{T}[s^{i},i=1,\dots,n]^{T}\\\
\beta(s,\bm{a})&=\bm{p}(\bm{a})^{T}[is^{i-1},i=1,\dots,n]^{T}\end{split}$ (24)

Observe that the non-holonomic constraints in the OCP (REF) accounted for in
the holonomic constraints in (24). The boundary constraints of (REF) are
satisfied because construction of the path satisfies path-state boundary
conditions, and construction of $\bm{p}(\bm{a})$ for any vector of way-points
$\bm{a}$ guarantees that
$\hat{a}(0)=\hat{\alpha}(s_{f})=0,\hat{v}(0)=v_{s},\hat{v}(s_{f})=v_{f}$.
Finally, for sufficiently large values of $M$, the boundary function
$\delta(\cdot,\cdot_{\text{max}})$ guarantees that $\bm{a}$ will only minimize
cost if $\alpha(s)\leq\alpha_{\text{max}},v(s)\leq
v_{\text{max}},\beta(s)\leq\beta_{\text{max}}$ for all $s\in[0,s_{f}]$.

Therefore at each iteration $k$ with way-points $\bm{a}^{k}$, the quadratic
sub-problem resulting in update-direction $\bm{d}$ is given by

$\min_{\bm{d}\in\mathbb{R}^{n}}\nabla
C(\bm{a}^{k})^{T}\bm{d}+\frac{1}{2}\bm{d}^{T}\nabla^{2}C(\bm{a}^{k})\bm{d}.$
(25)

Observe that this optimization problem is unconstrained. As a result, $\bm{d}$
is a solution if and only if

$\nabla^{2}C(\bm{a}^{k})\cdot\bm{d}+\nabla C(\bm{a}^{k})=0.$

##  VI Computing a Trajectory

Given

  1. 1.

$\rho_{0}=\rho_{\text{max}}$.

  2. 2.

$P_{0}=P(\rho_{0},\kappa_{\text{max}},\sigma_{\text{max}})$

  3. 3.

$v_{0}=\hat{v}(P_{0})$

  4. 4.

$Tr_{0}=(P_{0},v_{0})$

  5. 5.

$C_{0}=C(Tr_{0})$

  6. 6.

$C_{1}=-\infty$

  7. 7.

$i=1$

  8. 8.

while $C_{i-1}-C_{i}>$ tol do:

  9. 9.

`` $v=v_{i-1}$

  10. 10.

`` $\alpha,\beta,\gamma,\psi=\frac{d^{j}v}{ds^{j}},j=1,2,3,4$

  11. 11.

`` $\rho_{i}=$ solve (LABEL:Optv) for $\rho$

  12. 12.

`` $P_{i}=P(\rho_{i},\kappa_{\text{max}},\sigma_{\text{max}})$

  13. 13.

`` $v_{i}=\hat{v}(P_{i})$

  14. 14.

`` $Tr_{i}=(P_{i},v_{i})$

  15. 15.

`` $C_{i}=C(Tr_{i})$

  16. 16.

`` $i=i+1$

  17. 17.

end do

  18. 18.

return $Tr_{i}$

Proof: CHAING REASONING. TERMINATION BY PROBBLEM CONVEXITY. CONVEXTY BY
POSITIVE DEFINITENESS OF HESSIAN OF HAMILTONIAN OVER $\rho(s),v(s)$ FOR ANY
CHOICE!!!.

###  VI-A Velocity Sub-Problem

Given a maximum curvature acceleration $\rho_{\text{max}}$, a path
$P(\rho_{\text{max}})$ can be determined between start and goal path states
using the methods outlined in the previous section. In this section, we turn
our attention to computing a cost-minimizing velocity profile $v^{*}$ for the
path $P(\rho_{\text{max}})$. In particular, given a path
$P(\rho_{\text{max}})=\\{[x(s),y(s),\theta(s),\kappa(s),\rho(s)]^{T},s\in[0,s_{f}]\\}$,
we wish to solve the following optimal control problem:

$\begin{split}\min_{\beta(s)\in[-\beta_{\text{max}},\beta_{\text{max}}]}&C\big{(}P(\rho_{\text{max}}),v(s),\alpha(s),\beta(s)\big{)}\\\
s.t.\ &[\dot{v},\dot{\alpha}]=[\alpha,\beta]\\\
&\alpha\in[-\alpha_{\text{max}},\alpha_{\text{max}}],\
v\in(0,v_{\text{max}}]\\\
&v(0)=v_{s},v(s_{f})=v_{f},\alpha(0)=\alpha(s_{f})=0\end{split}$ (26)

Choosing $\beta$ as the control variable in (26) instead of $v$ ensures that
an optimal velocity profile $v^{*}(s)$ is at least twice differentiable with
respect to arc-length. Furthermore, observe that for any feasible solution
$\beta^{\prime}(s)$ there are unique speed and acceleration profiles
$v^{\prime}(s),\alpha^{\prime}(s)$ that satisfy the dynamic and boundary
constraints of (26). Indeed, $\beta(s)=\dot{\alpha}(s)$ fixes $\alpha(s)$ up
to a constant, while the constraint $\alpha(0)=0$, fix that constant (a
similar argument is used for velocity). Therefore, for the remainder of this
paper, when we refer to optimal profiles $\beta^{*}(s),\alpha^{*}(s),$ or
$v^{*}(s)$, we do so with the understanding that fixing any of these profiles
fixes the others.

######  Lemma VI.1.

Given a path $P(\rho_{\text{max}})$, there is at most one solution
$\beta^{*}(s)$ to the OCP in (26). Moreover, though (26) may not be an
instance of convex optimization (CO), it has no non-global local minima.
Finally, given two velocities $v_{1}(s),v_{2}(s)$, it holds that

$\begin{split}&||v_{1}(s)-v^{*}(s)||<||v_{2}(s)-v^{*}(s)||,\ \forall
s\in[0,s_{f}]\\\ &\Rightarrow
C(P(\rho_{\text{max}}),v_{1}(s),\alpha_{1}(s),\beta_{1}(s))\\\
<&C(P(\rho_{\text{max}}),v_{2}(s),\alpha_{2}(s),\beta_{2}(s)).\end{split}$
(27)

###### Proof.

We begin by showing that (26) possess a single local mimima $\beta^{*}$.
Observe that a necessary condition for any admissible controller
$\beta^{\prime}(s)$ to be a local minimizer of a cost functional is
$H_{\beta}|_{\beta=\beta^{\prime}}=0$ (see [liberzon2011calculus]). By (35),
there is a single function $\beta$ that fills this requirement, implying that
there is at most one extrema to

Let
$\bm{\lambda}^{*}=[\lambda_{k}^{*},\lambda_{\mathcal{J}}^{*},\lambda_{v}^{*},\lambda_{\alpha^{*}}]^{T}$
denote the vector of Lagrange multipliers associated with an optimal
controller $\beta^{*}$. ∎

##  VII Computing a Trajectory

In this section, we propose a method by which the bounds
$\rho_{\text{max}},\sigma_{\text{max}}$, and $\kappa_{\text{max}}$ may be
determined, and summarize how Trajectories can be computed. Let
$\rho_{\text{max}}^{\prime},\sigma_{\text{max}}^{\prime},\kappa_{\text{max}}^{\prime}$
denote the physical bounds of curvature acceleration, curvature rate, and
curvature, respectively. That is, the values that a human driver and vehicle
can achieve. Thus, for a particular trajectory from a start state to a goal
state, we wish to determine values of
$\rho_{\text{max}}\leq\rho_{\text{max}}^{\prime},\sigma_{\text{max}}\leq\sigma_{\text{max}}^{\prime},\kappa_{\text{max}}\leq\kappa_{\text{max}}^{\prime}$
which result in a trajectory with minimal cost $C$ given in (5).

To method we employ is as follows: first, we assume a simplified form of the
velocity profile as a function of arc-length along the trajectory. We then
determine the bounds that yield the minimal-cost trajectory given this
velocity profile. Once the bounds are determined, we can use the algorithm
from the previous section to compute a $G3$ path from start to goal path
states. Finally, the path from start to goal provides a curvature, curvature
rate, and curvature acceleration profile over the trajectory, which can then
be substituted into (LABEL:Optv) to determine the optimal velocity profile
given this curve.

Given a start and goal velocity $v_{s},v_{g}$, we use the following simplified
velocity profile for a given path whose final arc-length is $s_{f}$:

$\bar{v}(s)=\begin{cases}v_{s}+\frac{2(v_{f}-v_{s})s^{2}}{s_{f}^{2}}\
&\text{if }s\leq s_{f}/2\\\
0.5\left(v_{s}+v_{f}-\frac{(v_{f}-v_{s})(2s-s_{f})(2s-3s_{f})}{s_{f}^{2}}\right)\
&\text{otherwise}\end{cases}$ (28)

This is a velocity profile whose acceleration increases linearly until
$s_{f}/2$ and then decreases linearly with equal magnitude slope until
$s_{f}$. The result is an ”S” shaped curve with initial and final velocity and
acceleration given by $v_{s},0,v_{f},0$ respectively. We observe that this
velocity profile is therefore a feasible solution to the OCP in (9).

Next we assume that $\kappa_{\text{max}}=\kappa_{\text{max}}^{\prime}$. This
assumption is motivated by the fact that the $G3$ curves computed by the
algorithm from the previous section will not necessarily have top curvatures
equal to their maximum allowable value. Indeed, in Step 5, we compute the
minimum curvature that allows the curve in question to attain a desired final
heading. Therefore, the algorithm is in some sense self-correlating as it
determines top curvatures which are bounded by but not necessarily equal to
$\kappa_{\text{max}}$. We therefore need only determine
$\rho_{\text{max}},\sigma_{\text{max}}$. Currently, this is done by sampling
values of
$\rho_{\text{max}}\in(0,\rho_{\text{max}}^{\prime}],\sigma_{\text{max}}\in(0,\sigma_{\text{max}}^{\prime}]$,
and then computing the resulting paths using the algorithm provided in the
previous section. Once the final arc-length $s_{f}$ of the path is determined,
the cost of the resulting trajectory can then be computed using the assumed
velocity profile.

Finally, once the cost-minimizing values of
$\rho_{\text{max}},\sigma_{\text{max}}$ for the simplified velocity profile
are determined, the desired curvature, curvature rate, and curvature
acceleration profiles can be computed using (11). These profiles are then
substituted into (LABEL:Optv) in order to compute the optimal velocity profile
given this curve. Observe that this process could then continue recursively
resulting in the following algorithm:

Algorithm 2 Trajectory Generation

1:procedure TrajGen($\bm{p}_{s},\bm{p}_{g}$)

2: $\kappa_{\text{max}}=\kappa_{\text{max}}^{\prime}$

3: $v(s)=\bar{v}(s)$ in (28)

4:
$(\sigma_{\text{max}},\rho_{\text{max}})=\min_{(\rho_{\text{max}},\sigma_{\text{max}})}C$
where $C$ is given by (5) with $v=\bar{v}$

5: $P=$ path given $\sigma_{\text{max}},\rho_{\text{max}},\kappa_{\text{max}}$

6: $\bar{v}(s)=$ optimal speed profile from (LABEL:Optv) given path $P$

7: while Cost is decreasing do

8:
$(\sigma_{\text{max}},\rho_{\text{max}})=\min_{(\rho_{\text{max}},\sigma_{\text{max}})}C$
where $C$ is given by (5) with $v=\bar{v}$

9: $P=$ path given $\sigma_{\text{max}},\rho_{\text{max}},\kappa_{\text{max}}$

10: $\bar{v}(s)=$ optimal speed profile from (LABEL:Optv) given $P$

We observe that at each iteration of the while loop in Line 7 of this
algorithm, the compute bounds $\sigma_{\text{max}},\rho_{\text{max}}$ based on
an assumed velocity profile, and then update the profile based on
$\sigma_{\text{max}},\rho_{\text{max}}$. As such, the cost of the trajectory
determined at each iteration of the while loop is non-increasing.

In the next section, we use algorithm 3 with a pre-specified number of
iterations of the while loop in Line 7 to compute trajectories.

Algorithm 3 Trajectory Generation

1:procedure TrajGen($s_{f},\bm{p}(s),\bm{p}_{g}$)

2: sData $=[0=s_{0},s_{1},\dots,s_{n}=s_{f}]$

3: $\beta$Data $=[-\beta_{\text{max}},\dots,\beta_{\text{max}}]$

4: $G$ is graph

5: return A* on $G$ using heuristic = $H$

###### Proof that Greedy Never Backtracks.

You picked $\beta_{1}$, so cost to get to node plus cost from node to next
node plus cost of direct feasible to sf is less than for any other $\beta$.
Now at $s_{1}$ you expand. Because $\beta_{\text{direct}}$ is always an
option, the next beta you choose will have even less cost that the one you
picked. Therefore, if the one you picked was already better than everything
else, then so will the next one be.

##  VIII Results

In this section, we demonstrate the benefits of using our approach. We begin
by illustrating how the techniques developed herein can be used to anticipate
the expected driving styles of several archetypal users, namely, a comfort-
favoring (comfort) user who prefers low speed and high comfort, a moderate
user (equal speed and comfort), and a speed-favoring (speed) user (high speed,
low comfort). Next, we compare the theoretical cost of paths generated using
the proposed method to those from [banzhaf2018g]. As implied by Lemma III.2,
any path that solves (9) is bang-bang in the curvature acceleration $\rho$. It
is for this reason that we compare our methods for generating such $G3$ curves
to those proposed in [banzhaf2018g], the current state-of-the-art technique
for generating paths that are bang-bang in $\rho$. Finally, we measure
tracking error and final cost of our proposed trajectories using Matlab’s
seven degree-of-freedom simulator and built-in Stanley controller
[hoffmann2007autonomous]. The methods described here were encoded entirely in
Python 3.7 (Spyder), and the results were obtained using a desktop equipped
with an AMD Ryzen 3 2200G processor and 8GB of RAM running Windows 10 OS. We
begin with a note on the weights used in this section.

###  VIII-A Setup

Unit-less Weighting

The features in the cost function (7) represent different physical quantities.
Therefore, in order for this cost function to represent a meaningful trade-off
between these features for given weights, a scaling factor is included in the
weight [hogan2009sensitivity]. Similar to the scaling techniques in
[gulati2013nonlinear], we use a sub-optimal nominal velocity profile
$v_{\text{nom}}$ that allows us to roughly gauge the relative influence of
each feature on the cost. For a given path, the final arc-length, $s_{f}$ is
known, so the authors use a basic S-curve ramp from $v_{s}$ at $s=0$ to
$v_{f}$ at $s=s_{f}$ with minimal IS Jerk as $v_{\text{nom}}$. Once
$v_{\text{nom}}$ is chosen, the controller
$\beta_{\text{nom}}=\ddot{v}_{\text{nom}}$, and the resulting feature costs
$C^{\prime}_{a,\text{nom}},C^{\prime}_{\mathcal{J},\text{nom}},C^{\prime}_{y,\text{nom}},C^{\prime}_{t,\text{nom}}$,
and total cost
$C_{\text{nom}}=\sum_{m\in\\{a,\mathcal{J},y,t\\}}C^{\prime}_{m,\text{nom}}$
are calculated from (9). Finally, for given weights
$w_{a},w_{\mathcal{J}},w_{y},w_{t}$, we define the new scaled weights as

$\hat{w}_{m}=w_{m}\cdot\frac{C_{\text{nom}}}{4C^{\prime}_{m,\text{nom}}},\ \
m\in\\{a,\mathcal{J},y,t\\}.$

Interpreting $C_{\text{nom}}$ as a unit-less scalar, we observe that each term
$\hat{w}_{m}C^{\prime}_{m}$ in (7) will be unit-less for all $m$ and control
$\beta$. Moreover, the scaling factor
$C_{\text{nom}}/4C^{\prime}_{m,\text{nom}}$ naturally amplifies/reduces those
features whose costs are much lower/greater than the total nominal cost.

Parameter Bounds

For all experiments, the curvature parameter limits are given by
[banzhaf2018g]:

$\kappa_{\text{max}}=0.1982m^{-1},\ \sigma_{\text{max}}=0.1868m^{-2}\
\rho_{\text{max}}=0.3905m^{-3}.$ (29)

while the limits on velocity, acceleration, and instantaneous jerk are given,
respectively, by [bae2019toward]

$v_{\text{max}}=100km/hr,\ a_{\text{max}}=0.9m/s^{2},\
b_{\text{max}}=0.6m/s^{3}.$ (30)

###  VIII-B Evaluation

Qualitative Analysis

To qualitatively evaluate our methods, we analyse the trajectories for three
archetypal users: a speed user, who prefers low travel time over comfort, an
intermediate user who values comfort and speed equally, and a comfort user who
emphasizes comfort over speed. To simplify the comparison of these users, we
combine the scaled but unweighted features IS acceleration, IS jerk, and IS
yaw into a _discomfort cost_ , and compare this to _time cost_ for each user:

$\begin{split}\text{discomfort
cost}&=\sum_{m\in\\{a,\mathcal{J},y\\}}C_{m}^{\prime}\cdot\frac{C_{\text{nom}}}{4C^{\prime}_{m,\text{nom}}},\\\
\text{time
cost}&=C^{\prime}_{t}\cdot\frac{C_{\text{nom}}}{4C^{\prime}_{m,\text{nom}}.}\end{split}$

Given the simplistic nature of the speed, intermediate, and comfort
archetypes, the expected form of their favored trajectories is apparent: the
speed user wishes to minimize travel time at the expense of comfort.
Therefore, she will favor trajectories featuring short paths and high
velocities. The comfort user, on the other hand, prefers comfortable
trajectories over short travel times. Finally, the intermediate driver should
favor a balance of the two other users. In this experiment, the start-goal
configurations for each user are:

$p_{s}=[0,0,0,0],\ p_{f}=[30,105,0,0.1695],$

with $v_{s}=v_{g}=7m/s$. The weights for each user are:

$\begin{split}\text{speed User}\
&w_{t}=0.9,w_{y}=w_{\mathcal{J}}=w_{a}=0.0333,\\\ \text{Intermediate User}\
&w_{t}=w_{y}=w_{\mathcal{J}}=w_{a}=0.25\\\ \text{comfort User}\
&w_{t}=0.01,w_{y}=w_{\mathcal{J}}=w_{a}=0.33.\end{split}$ (31)

Thus, the speed user places a high weight on time, and a small weight on IS
yaw rate, IS Jerk, and IS acceleration (i.e., small weight on discomfort),
while the comfort user places a low weight on time, and the intermediate user
places an equal weight on all features.

Figure 9: Comparison of three users. Left: velocity (top), time cost (mid) and
discomfort cost (bottom) for three users. Red, blue green curves represent
speed, intermediate, and comfort users respectively Right: trajectories of the
tree users. The longest trajectory is that of the comfort user, the shortest
is that of the speed. Here, color represents velocity.

The results are illustrated in Figure 9. The left image illustrates the
velocity profile (top), time cost (mid), and discomfort cost (bottom) for the
speed (red), intermediate (blue), and comfort (green) drivers. The right image
illustrates the resulting trajectories for each user with color representing
velocity. The shortest trajectory is that of the speed user. Observe that her
path is short and velocities high, reflecting her preference towards short
travel times. The longest trajectory is that of the comfort user, and we note
that her trajectory features consistently low velocities and a long path. This
tendency towards longer path length, allows the comfort user to change
velocities over a longer time period allowing her to reach lower velocities
with lower acceleration and jerk. This is reflected in the low discomfort cost
of the comfort user. The middle trajectory is that of the intermediate user,
who decreases her velocity on the turn to minimize jerk/acceleration, but then
speeds up again on the straight portion of the path.

Path Evaluation

We now evaluate the quality of the paths generated using our technique. To
this end, we isolate the effect of the path on the final cost by comparing two
methods for generating trajectories:

  * •

_Method 1_ : Paths and velocity profiles are computed using the methods
detailed in this paper.

  * •

_Method 2_ : velocity profiles are computed using the techniques outlined
here, but paths are computed from [banzhaf2018g].

We define the $\%$ Savings as the difference in cost (given in (7)) for
trajectories generated using Method 2 and Method 1 as a percentage of the cost
of Method 2-based trajectories.

For this experiment, 1300 start-goal pairs were randomly generated with
initial and final curvatures ranging from $-\kappa_{\text{max}}$ to
$\kappa_{\text{max}}$, initial and final velocities (taken to be equal)
ranging from $0$ to $v_{\text{max}}$, initial and final $x,y$-values ranging
from $-20m$ to $20m$, and weights ranging from 0 to 1. Initial and final
velocities were chosen to reflect the Euclidean distance between start-goal
pairs, and pairs with no feasible solution were disregarded. The results of
these experiments are tabulated in Table I. Here, a feature (IS time, IS yaw,
IS acceleration, IS jerk) is said to be _dominant_ if the corresponding weight
is greater than 0.5. If no weight is greater than 0.5, the features are said
to be _blended_. It was found that the average percent savings was $33.10\%$
across all experiments. Moreover, if the velocity is not optimized using our
method, but is assumed to be constant instead, we see see a $45.5\%$ savings.
As Table I implies, the largest savings is typically found when IS jerk is the
dominant feature, and when initial and final velocities are medium to high.
This implies that the techniques presented here have greater value in highway
driving scenarios (where velocities are high, and high values of jerk can lead
to dangerous maneuvers) than driving in a parking-lot, but can still reduce
costs by around $25\%$ on average. A detailed view of the distribution of the
percent savings for the experiments is given in Figure 10. Observe that the
instances where the percent savings are substantial $(>50\%)$ are not
infrequent. In particular, in $32\%$ of experiments, the savings was at least
$50\%$, while in $26\%$ of experiments, the savings was at least $70\%$, and
$17\%$ of experiments had a savings of at least $80\%$.

|  | Avg. Percent Savings (%)
---|---|---
Dominant Feature | IS Time | 23.04 (31.4)
IS Yaw | 28.69 (32.4)
IS Acceleration | 27.66 (32.5)
IS Jerk | 42.81 (35.1)
Blended | 39.36 (35.9)
Initial/Final Velocity | Low $(0,v_{\text{max}}/3]$ | 24.38 (31.9)
Med $(v_{\text{max}}/3,2v_{\text{max}}/3]$ | 41.74 (35.5)
High $(2v_{\text{max}}/3,v_{\text{max}}]$ | 39.71 (34.3)
TABLE I: Average cost savings. Breakdown by dominant feature and initial/final
velocity. Figure 10: Percent savings distribution by initial/final velocity
(top), and dominant feature (bottom).

Tracking Error and Simulation Cost

As a final validation of our work, we consider three metrics: maximum tracking
error, average tracking error, and cost. We again consider Method 1 and Method
2 defined above. The experiments in this section were carried out using
MATLAB’s seven degree of freedom driving simulator in its autonomous driving
toolbox. The reference trajectories were tracked using MATLAB’s Stanely
controller with default gains. Here, the maximum error is the maximum lateral
error, and the average error is the integral of the lateral error normalized
to arc-length of the path. Percent savings for each metric were computed for
100 randomly generated start-goal pairs with random weights, and initial/final
velocities. The distribution of the savings is illustrated in Figure 11. The
mean percent savings of cost, average error, and maximum error were
$26.22\%,15\%$, and $13.30\%$, respectively. Table II illustrates the
percentage of experiments that resulted in savings of at least 30, 50, or 70
percent for each metric. Notably, Table II implies that in 35.11, 25.53, and
15.96 percent of start-goal pairs, we can expect at least 30 percent savings
in cost, average, and maximum errors, respectively.

Figure 11: Cost, maximum error and average error $\%$ savings. | Frequency ($\%$) with savings exceeding
---|---
Metric |  $30\%$ |  $50\%$ |  $70\%$
Cost |  35.1 |  27.7 |  14.9
Average Error |  25.3 |  4.3 |  2.1
Maximum Error |  16.0 |  6.4 |  1.1
TABLE II: Distribution of $\%$savings for cost, average error, and maximum
error .

Run-time

The focus of this work was on improving the quality of computed trajectories
given user weights. As such, we did not focus on optimizing for run time.
However, we believe that with proper software engineering, one could
substantially reduce the run-time of our procedure though it may still exceed
the state-of-the-art (SOTA).

|  | Run-times (s)
---|---|---
Sub-routine | SOTA source | SOTA (C) | Ours (Python)
Calculate a path (Step 1) | [banzhaf2018g] | $6.8(10^{-5})$ | $4.3(10^{-3})$
Optimize velocity (Step 2) | [4650924] | $2.2(10^{-4})$ | $4.1(10^{-2})$
Total (recursive Step 1 & 2) |  [banzhaf2018g] & [4650924] | $1.2(10^{-3})$ | $5.1(10^{-1})$
TABLE III: Comparison of run-times. Our methods (Ours) compared against state-
of-the-art (SOTA) for each sub-routine of the procedure.

Run-times for each of the two steps in our procedure are compared with those
of the SOTA in Table III which also includes the total time. Reference
[4650924] as the SOTA for velocity profile planning given a path due to its
use of third order trapezoidal methods. Trapezoidal methods (and variants
thereof) are a widely used method for velocity profile planning
[fang2019smooth, 8843215]. Variants of this method use higher-order velocity
models than those employed by [4650924], improving the quality at the expense
of computation time. We therefore use computation times reported in [4650924]
as a lower bound on trapezoidal velocity method variants. We observe that the
SOTA is consistently on the order of 100 times faster than he methods proposed
here. However, it should be noted that our algorithm is encoded entirely in
Python which tends to run 10-100 times slower than C for iterative procedures
like ours.

NOTES: 1) compare velocity to basic trapezoidal method over a given curve. 2)
Mention that this technique reduces decision variables in path planning to
just $\rho$. Add velocity citations ([4650924]) to intro.

##  Appendix A Proofs

###### Proof of Theorem LABEL:them:optspd.

Let $\alpha=dv/ds,\ \beta=d\alpha/ds$ represent arc length parameterized
acceleration, and jerk respectively. Observe,

$\begin{split}a_{N}&=v^{2}\kappa,\
a_{T}=\frac{dv}{dt}=\frac{dv}{ds}\frac{ds}{dt}=\alpha v,\\\
\mathcal{J}_{N}&=\frac{da_{N}}{dt}=\frac{da_{N}}{ds}\frac{ds}{dt}=v^{2}(2\alpha\kappa+v\sigma),\\\
\mathcal{J}_{T}&=\frac{d}{dt}\left(\frac{dv}{dt}\right)=v\frac{d}{ds}\left(\alpha
v\right)=v(\beta v+\alpha^{2}),\\\
C_{y}&=\frac{d\theta}{dt}=\frac{d\theta}{ds}\frac{ds}{dt}=\kappa
v.\end{split}$ (32)

Replacing (32) in (5), and integrating with respect to $s$ instead of $t$,
yields

$C=\int_{0}^{s_{f}}w_{a}C_{a}+w_{\mathcal{J}}C_{\mathcal{J}}+w_{y}C_{y}+w_{t}C_{t}ds$

where

$\begin{split}C_{a}&=v^{3}\kappa^{2}+\alpha^{2}v\\\
C_{\mathcal{J}}&=v^{3}(2\alpha\kappa+v\sigma)^{2}+v(\beta v+\alpha^{2})^{2}\\\
C_{y}&=\kappa^{2}v\\\ C_{t}&=v^{-1}.\end{split}$ (33)

This re-definition of the cost function in terms of arc-length $s$, instead of
time results in a new OCP whose Hamiltonian is given by

$\begin{split}H=&w_{a}C_{a}+w_{\mathcal{J}}C_{\mathcal{J}}+w_{y}C_{y}+w_{t}C_{t}\\\
&+\lambda_{\kappa}\sigma+\lambda_{\sigma}\rho+\lambda_{v}\alpha+\lambda_{\alpha}\beta\\\
&+\lambda_{x}\cos(\theta)+\lambda_{y}\sin(\theta)+\lambda_{\theta}\kappa,\end{split}$
(34)

For Lagrange multipliers
$\lambda_{\kappa},\lambda_{\sigma},\lambda_{v},\lambda_{\alpha},\lambda_{x},\lambda_{y},\lambda_{\theta}$.
Let $\bm{\lambda}$ be a vector of the Lagrange multipliers, let $\bm{x}$ be a
vector of states, and let the suffix ${}^{\prime}*^{\prime}$ written after a
value represent the cost-minimizing version of that value. Then, Pontryagin’s
Minimum Principle (PMP) states, in part, that if $u$ represents a controller,
then

$H(\bm{\lambda}^{*},\bm{x}^{*},\bm{u}^{*})\leq
H(\bm{\lambda}^{*},\bm{x}^{*},\bm{u}),\ \forall\bm{u}\in U$

Thus, we attempt to minimize $H$ in (34) over $\beta$:

$\begin{split}\frac{\partial
H}{\partial\beta}&=\lambda_{\alpha}+2w_{\mathcal{J}}v^{2}(\alpha^{2}+v\beta),\\\
\frac{\partial^{2}H}{\partial\beta^{2}}&=2w_{\mathcal{J}}v^{3}.\end{split}$
(35)

Pontryagin’s minimum principle, together with (35), imply that if there is a
cost-minimizing controller, $\beta$, then this controller is unique as there
is a unique. Indeed, by (35), the Hamiltonian has a single critical value of
$\beta$, and its second derivative is positive everywhere assuming
$w_{\mathcal{J}},v>0$. Moreover, (35) allows us to conclude that

$\frac{\partial H}{\partial\beta}=0\ \Rightarrow\
\lambda_{\alpha}=-2w_{\mathcal{J}}v^{2}\big{(}\alpha^{2}+v\beta\big{)}.$ (36)

We therefore obtain a theoretical expression for the Lagrange multiplier
$\lambda_{\alpha}$, which we may differentiate with respect to arc-length:

$\dot{\lambda}_{\alpha}=-2w_{\mathcal{J}}v\left(\gamma
v^{2}+2\alpha^{3}+5\alpha\beta v\right).$ (37)

However, from Hamiltonian dynamics, we know that

$\dot{\lambda}_{\alpha}=-\frac{\partial H}{\partial\alpha}.$ (38)

Equating equations (37) and (38), and solving the result for $\lambda_{v}$, we
obtain

$\lambda_{v}=2v\left(w_{\mathcal{J}}v^{2}\big{(}-4\alpha\kappa^{2}-2\kappa\sigma
v+\gamma\big{)}+\alpha\big{(}3w_{\mathcal{J}}v\beta-w_{a}\big{)}\right).$ (39)

Again, we may differentiate $\lambda_{v}$ with respect to $s$, and equate the
result to $\dot{\lambda}_{v}=-(\partial H)/(\partial v)$, to obtain the result
of the Theorem. ∎

###### Proof of Theorem LABEL:thm:optcurve.

From (34), we observe that

$\frac{\partial H}{\partial\rho}=\lambda_{\sigma},$

implying that $H$ is linear in the variable $\rho$. Thus by PMP, we may
concluded that the cost-minimizing function $\rho$ is bang-bang, taking values
$\rho_{\text{max}}$ when $\lambda_{\sigma}\leq 0$, and $-\rho_{\text{max}}$
when $\lambda_{\sigma}>0$. That is, the optimal form of $\rho$ is a piece-wise
constant, implying that the optimal $\sigma$ is piece-wise linear, i.e., a
$G3$ curve whose curvature acceleration takes, in magnitude, its maximum
permissible value $\rho_{\text{max}}$ ∎

###### Proof of Lemma IV.1.

by equations (1), (11), the final heading of the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\Delta)$ is given by

$\begin{split}P_{\theta}^{s_{6}}&(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)=\int_{0}^{s_{6}}\kappa
ds\\\ &=\int_{0}^{s_{3}}\kappa ds+\int_{s_{3}}^{\Delta}\kappa
ds+\int_{\Delta}^{s_{6}}\kappa ds,\\\ &=\int_{0}^{s_{3}}\kappa
ds+\kappa_{\text{top}}(\Delta-s_{3})+\int_{\Delta}^{s_{6}}\kappa
ds,\end{split}$

for all $\Delta\geq s_{3}$. We observe (though the proof is omitted for
brevity) that by (10),

$\frac{d}{d\Delta}\int_{0}^{s_{3}}\kappa
ds=\frac{d}{d\Delta}\int_{\Delta}^{s_{6}}\kappa=0.$

Thus for all $\Delta\geq s_{3}$,

$P_{\theta}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)=K+\kappa_{\text{top}}(\Delta-
s_{3}),$

where $K$ is a constant with respect to $\Delta$. Therefore, if it is desired
that
$P_{\theta}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)=\theta_{g}$,
then it must hold that

$\theta_{g}=K+\kappa_{\text{top}}(\Delta-
s_{3})\Rightarrow\Delta=(\theta_{g}-K)\kappa_{\text{top}}^{-1}+s_{3}.$

Finally, observing that

$P_{\theta}^{s_{6}}(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta=s_{3})=K+\kappa_{\text{top}}(s_{3}-s_{3})=K$

completes the proof. ∎

###### Proof Of Theorem IV.2.

Let $\bm{p}^{\prime}=(x^{\prime},y^{\prime},\theta^{\prime},\kappa^{\prime})$
denote the path states along the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta=s_{3})$ which is the
element of the set $\mathcal{P}$ where $\Delta=s_{3}$. For any other element
of the set $\mathcal{P}$, let $\bm{p}=(x,y,\theta,\kappa)$ denote its path
states. Finally, let the subscript $f$ represent the terminal value of any
state.

Note that it suffices to prove that the shortest distance from the point
$\bm{x}_{c}$ given in (13) to the line passing through $(x_{f},y_{f})$ whose
slope is $\tan{z_{f}}$ does not depend on $\Delta$. We observe that, by
construction,

$\kappa(s)=\begin{cases}\kappa^{\prime}(s),\ &\text{if }s\leq s_{3}\\\
\kappa_{\text{top}},\ &\text{if }s_{3}<s\leq\Delta\\\
\kappa^{\prime}(s-(\Delta-s_{3})),\ &\text{if }\Delta<s\leq s_{6}.\end{cases}$

Figure 12: Curvature profile of two curves in the set $\mathcal{P}$. (Red):
curvature of the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta=s_{3})$. (Black):
curvature of the curve $P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$
for $\Delta>s_{3}$.

This definition becomes evident in Figure 12. Integrating this curvature
function results in the following heading function:

$\theta(s)=\begin{cases}\theta^{\prime}(s),\ &\text{if }s\leq s_{3}\\\
\theta^{\prime}(s_{3})+\kappa_{\text{top}}(s-s_{3}),&\text{if
}s_{3}<s\leq\Delta\\\ \kappa_{\text{top}}(\Delta-
s_{3})+\theta^{\prime}(s-\Delta+s_{3}),\ &\text{if }\Delta<s\leq
s_{6}.\end{cases}$

This implies that $\theta_{f}$ can be expressed as a function of $\Delta$ as:

$\theta_{f}(\Delta)=\kappa_{\text{top}}(\Delta-
s_{3})+\theta^{\prime}(s_{6}-\Delta+s_{3})=\theta_{f}^{\prime}+\kappa_{\text{top}}(\Delta-
s_{3}).$

Finally, integrating the cosine and sine of $\theta(s)$ from $s=0$ to
$s=s_{6}$, we obtain the final $x,y$ position of the curve
$P(\bm{p}_{s},\kappa_{\text{top}},\kappa_{g},\Delta)$ as functions of
$\Delta$:

$\begin{split}x_{f}(\Delta)=&x^{\prime}(s_{3})-\frac{\sin(\theta(s_{3}))-\sin(\kappa_{\text{top}}(\Delta-
s_{3})+\theta(s_{3}))}{\kappa_{\text{top}}}\\\
&+\cos(\kappa_{\text{top}}(\Delta-s_{3}))(x_{f}^{\prime}-x^{\prime}(s_{3}))\\\
&-\sin(\kappa_{\text{top}}(\Delta-s_{3}))(y_{f}^{\prime}-y_{3}),\\\
y_{f}(\Delta)=&y^{\prime}(s_{3})-\frac{-\cos(\theta(s_{3}))+\cos(\kappa_{\text{top}}(\Delta-
s_{3})+\theta(s_{3}))}{\kappa_{\text{top}}}\\\
&+\sin(\kappa_{\text{top}}(\Delta-s_{3}))(x_{f}^{\prime}-x^{\prime}(s_{3}))\\\
&+\cos(\kappa_{\text{top}}(\Delta-s_{3}))(y_{f}^{\prime}-y_{3}).\end{split}$

Observe now that the shortest distance from the point
$\bm{x}_{c}=[x_{c},y_{c}]^{T}$ given in (13) to the line passing through
$(x_{f}(\Delta),y_{f}(\Delta))$ whose slope is $\tan{z_{f}}$, is given by

$d(\Delta)=\frac{x_{c}\tan{z_{f}(\Delta)}-y_{c}+y_{f}(\Delta)-x_{f}(\Delta)\tan{z_{f}(\Delta)}}{\sqrt{\tan{z_{f}(\Delta)}^{2}+1}}.$

Replacing the functions $x_{f}(\Delta),y_{f}(\Delta),\theta_{f}(\Delta)$ in
$d(\Delta)$, and differentiating the result with respect to $\Delta$ yields 0.
Finally noting that $d(\Delta)$ is a continuous function of $\Delta$, we may
conclude that $d(\Delta)$ is constant for all values of $\Delta\geq s_{3}$ as
desired. ∎

###### Proof of Theorem IV.5.

We will begin by considering four new continua:

$\begin{split}\bar{S}^{+}&=\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)}\in[0,\kappa_{\text{max}}),\kappa_{g}=0,\Delta^{(1)}=s_{3}^{(1)})\\}\\\
&\dot{\bigcup}\
\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)}=\kappa_{\text{max}},\kappa_{g}=0,\Delta^{(1)}\geq
s_{3}^{(1)})\\}\\\
\bar{S}^{-}&=\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)}\in(-\kappa_{\text{max}},0],\kappa_{g}=0,\Delta^{(1)}=s_{3}^{(1)})\\}\\\
&\dot{\bigcup}\
\\{P(\bm{p}^{(1)},\kappa_{\text{top}}^{(1)}=-\kappa_{\text{max}},\kappa_{g}=0,\Delta^{(1)}\geq
s_{3}^{(1)})\\}\\\
\bar{G}^{+}&=\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)}\in[0,\kappa_{\text{max}}),\kappa_{g}=0,\Delta^{(2)}=s_{3}^{(2)})\\}\\\
&\dot{\bigcup}\
\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)}=\kappa_{\text{max}},\kappa_{g}=0,\Delta^{(2)}\geq
s_{3}^{(2)})\\}\\\
\bar{G}^{-}&=\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)}\in(-\kappa_{\text{max}},0],\kappa_{g}=0,\Delta^{(2)}=s_{3}^{(2)})\\}\\\
&\dot{\bigcup}\
\\{P(\bm{p}^{(2)},\kappa_{\text{top}}^{(2)}=-\kappa_{\text{max}},\kappa_{g}=0,\Delta^{(2)}\geq
s_{3}^{(2)})\\}.\end{split}$ (40)

Each of these continua is the union of two sets. The first set is comprised of
curves whose top curvature has magnitude strictly less than the maximum
allowable value, and minimum possible values of $\Delta^{(i)},i=1,2$, while
the second set is comprised of curves whose top curvatures have magnitude
equal to the maximum allowable value and values of $\Delta$ that are greater
than or equal to their minimum allowable value. We make two observations:

  1. 1.

The shortest path connecting the continua $S^{\pm},G^{\pm}$ is formed by
connecting a curve $P1\in\bar{S}^{\pm}$ with a curve $P2\in\bar{G}^{\pm}$
using a straight line.

  2. 2.

If a path is returned at the end of Step 6, then it is comprised of a curve
$P1\in\bar{S}^{\pm}$ connected to a curve $P2\in\bar{G}^{\pm}$ with a straight
line. Furthermore this path is of minimal arc-length connecting continua
$\bar{S}^{\pm},\bar{G}^{\pm}$.

Note that it suffices to prove that these two observations are correct.
Indeed, if the second observation is correct, then the path returned by Steps
1-6 is the path of smallest arc-length connecting continua
$\bar{S}^{\pm},\bar{G}^{\pm}$. By the first observation, the path of minimum
arc-length between start and goal path states must connect the continua
$\bar{S}^{\pm},\bar{G}^{\pm}$. Therefore, if a path is returned after Step 6
of the algorithm, then it is the shortest path comprised of $G3$ curves
described in this section between start and goal path states.

We will prove the first observation by contradiction. Suppose the arc-length
minimal path connecting $S^{\pm},G^{\pm}$ was not comprised of a curve
$P1\in\bar{S}^{\pm}$ and a curve $P2\in\bar{G}^{\pm}$ connected with a
straight line. Then either $P1\in S^{\pm}-\bar{S}^{\pm}$ or $P2\in
G^{\pm}-\bar{G}^{\pm}$. Without loss of generality, suppose $P1\in
S^{\pm}-\bar{S}^{\pm}$. Then
$|\kappa_{\text{top}}^{(1)}|<\kappa_{\text{max}},\Delta^{(1)}>s_{3}^{(i)}$.
Let $\theta_{f}^{(1)}$ denote the final heading of the curve $P1$, and observe
that for $\epsilon,\delta>0$ we can increment the magnitude of the top
curvature $|\kappa_{\text{top}}^{(1)}|=|\kappa_{\text{top}}^{(1)}|+\epsilon$,
and decrement the length spent at the top curvature
$\Delta^{(1)}=\Delta^{(1)}-\delta$ in such a way as to maintain the final
heading (using equation (16)). Observe further that by doing so, according to
(10), we will decrease the final arc-length $s_{6}^{(1)}$. Thus for any curve
$P1$ – and any final heading of $P1$ – there always exists a shorter curve
with the same final heading unless $P1\in\bar{S}^{\pm}$.

If a path is returned at the end of Step 6 with $\theta_{\text{error}}=0$, the
we observe that $\theta_{\text{exit}}=\theta_{\text{connect}}$ implying that
the two curves $P1,P2$ returned after Step 6 can be connected with a straight
line whose slope is given by $\theta_{\text{exit}}$. In Step 5, we determine
values $\kappa_{\text{top}}^{(i)},\Delta^{(i)}$ such that the final heading of
curve $Pi,i=1,2$ is equal to $\theta_{\text{exit}}+\pi(i-1)$. In particular,
this step begins by assuming $\Delta^{(i)}=s_{3}^{(i)}$, and determines the
required value of $\kappa_{\text{top}}^{(i)}$, if the resulting top curvature
solution is too big, this step then sets
$\kappa_{\text{top}}^{(i)}=\pm\kappa_{\text{max}}$, and increases the value
$\Delta^{(i)}$ using equation (16). Therefore, the curve $Pi,i=1,2$ will
either have $|\kappa_{\text{top}}^{(i)}|=\kappa_{\text{max}}$ or
$\Delta^{(i)}=s_{3}^{(i)}$ implying that it is a curve in $\bar{S}^{\pm}$ if
$i=1$ or $\bar{G}^{\pm}$ if $i=2$.

Finally, to see why the curves $P1,P2$ have minimal arc-length, we observe
that the condition (19) admits at most one solution for the exiting angle
$\theta_{\text{exit}}$ modulo $2\pi$ that allows $P1$ and $P2$ to be connected
with a straight line. Because Step 5 begins by computing the smallest
magnitude $\kappa_{\text{top}}^{(i)}$ such that the final heading of $Pi$ is
$\theta_{\text{exit}}+\pi(i-1)$, we know that there is no smaller top
curvature, and thus no shorter curve by (10), that achieve the desired final
heading. This concludes the proof. ∎

##  Appendix B Quasi-Convexity

Let

$\displaystyle J(u)=\int_{0}^{t_{f}}L(x,y,u)dt$ (41)
$\displaystyle[\dot{x},\dot{y}]=[y,u]$ (42) $\displaystyle h(x,y,u)\leq 0$
(43) $\displaystyle[x(t_{f}),y(t_{f})]=[x_{f},y_{f}]$ (44)
$\displaystyle[x(0),y(0)]=[x_{0},y_{0}]$ (45)

######  Lemma B.1.

If $L$ is a quasi-convex, non-negative function of $x,y,u$, the function $h$
is convex, and $t_{f}$ is fixed, then $J(u)$ is a quasi-convex function of
$u$.

###### Proof.

Let $u=\lambda_{1}u_{1}+\lambda_{2}u_{2}$ where $u_{1},u_{2}$ are feasible
controllers, $\lambda_{1},\lambda_{2}\in[0,1]$, $\lambda_{1}+\lambda_{2}=1$,
and $J(u_{1}),J(u_{2})\leq J$ for $J\in\mathbb{R}$. We must show that
$J(u)\leq J$. Begin by observing that $u$ is feasible. Indeed, let
$x_{i},y_{i}$ denote the states associated with $u_{i},i=1,2$. Observe

$\begin{split}x&=\lambda_{1}x_{1}+\lambda_{2}x_{2}\\\
y&=\lambda_{1}y_{1}+\lambda_{2}y_{2}\end{split}$ (46)

satisfy the constraints (42)$\rightarrow$(45) for $u$ given above:

$\begin{split}\text{\eqref{dyn}:
}&\dot{x}=\lambda_{1}\dot{x}_{1}+\lambda_{2}\dot{x}_{2}=\lambda_{1}y_{1}+\lambda_{2}y_{2}=y\\\
&\dot{y}=\lambda_{1}\dot{y}_{1}+\lambda_{2}\dot{y}_{2}=\lambda_{1}u_{1}+\lambda_{2}u_{2}=u\\\
\text{\eqref{bounds}:
}&h(x,y,u)\leq\lambda_{1}h(x_{1},y_{1},u_{1})+\lambda_{2}h(x_{2},y_{2},u_{2})\leq
0\\\ &\text{by the convexity of }h\text{ and feasibility of }u_{i},i=1,2\\\
\text{\eqref{boundaryg}:
}&x(t_{f})=\lambda_{1}x_{1}(t_{f})+\lambda_{2}x_{2}(t_{f})=x_{f}\\\
&y(t_{f})=\lambda_{1}y_{1}(t_{f})+\lambda_{2}y_{2}(t_{f})=y_{f}\\\ &\text{by
feasibility of }u\text{ and }\lambda_{1}+\lambda_{2}=1\\\
\text{\eqref{boundarys}: }&\text{Similar to \eqref{boundarys}.}\end{split}$

Now that we have established that $u$ is feasible, we must now show that
$J(u)\leq J$. Suppose WLOG, that $J(u_{1})>J(u_{2})$. Then it suffices to show
that $J(u)\leq J(u_{1})$. Suppose that the cost in (41) is changed slightly:
let

$J(u,t)=\int_{t}^{t}L(x,y,u)d\tau$

If $J(u_{1},t),J(u_{2},t)\leq J(t)$ for all $t\in[0,t_{f}]$, then $J(u,t)\leq
J(t)$ for all $t\in[0,t_{f}]$. Indeed,

$\begin{split}&J(u,t)=\int_{0}^{t}L(x,y,t)d\tau\leq J(t)\\\ &\Leftarrow
L(x,y,t)\leq\dot{J}(t)\\\ &\Leftarrow
L(x_{1},y_{1},t),L(x_{t},y_{2},t)\leq\dot{J}(t)\\\ &\Leftarrow J(u_{i},t)\leq
J(t),i=1,2\end{split}$ (47)

This comes from the observation that $J(u_{i},0)=J(u,0)=0$.

Suppose that $J(u)>J$. Then there must exist a
$u^{\prime}=\lambda_{1}u_{1}+\lambda_{2}u_{2}$ for some $\lambda_{i}$ that is
locally maximal. That is, there must exist a $u^{\prime}$ such that for any
$\eta(t)$ continuous, there exists a $\epsilon$ suficciently small that
$u^{\prime}+\epsilon\eta(t)$ is feasible, and
$J(u^{\prime})>J(u^{\prime}+\epsilon\eta(t))$. Observe now that

$J(u^{\prime}+\epsilon\enta(t))=J(u^{\prime})+\frac{d}{du}(u)\eta(t)$

∎

######  Lemma B.2.

The cubic polynomial $P^{3}(s)=p_{0}+p_{1}s+p_{2}s^{2}+p_{3}s^{3}$ evaluated
on the interval $[a,b]$ can be approximated using the quadratic polynomial

$\begin{split}P_{\text{approx}}(s)&=p_{0}^{\prime}+p_{1}^{\prime}s+p_{2}^{\prime}s^{2},\
\text{where}\\\ p_{0}^{\prime}&=\frac{ab(a+b)p_{3}}{2}+p_{0}\\\
p_{1}^{\prime}&=-\frac{(a^{2}+4ab+b^{2})p_{3}}{2}+p_{1}\\\
p_{2}^{\prime}&=\frac{3(a+b)p_{3}}{2}+p_{2}\end{split}$ (48)

with a maximum error of

$\max_{s\in[s_{1},s_{2}]}|P^{3}(s)-P_{\text{approx}}(s)|=\frac{\sqrt{3}p_{3}}{36}(b-a)^{3}.$
(49)

Furthermore, $P_{\text{approx}}(a)=P^{3}(a),P_{\text{approx}}(b)=P^{3}(b)$.