# Minimal $t-$spanning Primitive Sets: An MILP Formulation

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>
# High-Dimensional Lattice Planning with Optimal Motion Primitives

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>
###### Abstract

Lattice-based planning techniques simplify the motion planning problem for
autonomous vehicles by limiting available motions to a pre-computed set of
_primitives_. These primitives are then combined online to generate more
complex maneuvers. A set of motion primitives $t$-span a lattice if, given a
real number $t\geq 1$, any configuration in the lattice can be reached via a
sequence of motion primitives whose cost is no more than a factor of $t$ from
optimal. Computing a minimal $t$-spanning set balances a trade-off between
computed motion quality and motion planning performance. In this work, we
formulate this problem for an arbitrary lattice as a mixed integer linear
program. We also propose an A*-based algorithm to solve the motion planning
problem using these primitives. Finally, we present an algorithm that removes
the excessive oscillations from planned motions – a common problem in lattice-
based planning. Our method is validated for autonomous driving in both parking
lot and highway scenarios.

##  I Introduction

Broadly, the motion planning problem is to plan a collision free motion
between configurations of a vehicle and optimizes desirable properties like
travel time or comfort [1]. The motion can be used as a reference for a
tracking controller [2] for a fixed amount of time before a new motion is
planned.

In general, the motion planning problem—the focus of this work—is intractable
[3] owing in part to potentially complex vehicle constraints that impede the
calculation of two point boundary value (TPBV) problems. Therefore,
simplifying assumptions are made. The variety in possible simplifications has
given rise to several planning techniques. These typically fall into one of
four categories [4]: sampling based planners, interpolating curve planners,
numerical optimization approaches, and graph search based planners.

Lattice-based motion planning is an example of a graph search planner and is
one of the most common approaches to solving the motion planning problem[4,
5]. It works by discretizing the vehicle’s configuration space into a
countable set (or _lattice_) of regularly repeating configurations.
Kinodynamically feasible motions between lattice configurations (or
_vertices_) are pre-computed and a subset of these motions called a _control
set_ is selected [5, 6, 7, 8]. Elements of this control set can then be
concatenated in real time to form complex maneuvers. Lattice-based planners
simplify the motion planning problem by limiting the set of available motions
instead of simplifying their kinodynamics – guaranteeing feasibility.
Computationally expensive TPBV problems are solved offline between lattice
vertices. Further, the regularity of the lattice structure allows for
theoretical guarantees on the cost of lattice-optimal paths compared to free
space optimal under certain assumptions [9]. These guarantees do not require
the fineness of the lattice grid structure to approach infinity, which is
typically required in sampling-based techniques. However, lattice-based motion
planning is not without its critiques. This work presents novel solutions for
three such critiques:

Figure 1: Motion planning using $t$-spanning $G3$ motion primitives. Magenta:
primitives used, Red: final motion, Cyan: car footprint.

####  I-1 Choice of Control Set

It is observed in [10, 11] that the _number_ of primitives in a control set
favorably affects the quality of the resulting paths but adversely affects the
run-time of an online search. The authors of [10] introduce the notion of a
$t$-spanning control set: a control set that guarantees the existence of
compound motions – motions formed by concatenating motion primitives – whose
costs are no more than a factor of $t$ from optimal. They propose to find the
smallest set that, for a given value $t>1$, will $t$-span a lattice thus
optimizing a trade-off between path quality and performance. Computing such a
control set is called the Minimum $t$-Spanning Control Set (MTSCS) problem
which is known to be $NP$-hard [11]. Hitherto, no non brute-force solution to
the MTSCS problem had been proposed.

####  I-2 Continuity in All States & On-lattice Propagation

Given a kinodynamically feasible motion originating from an initial
configuration $p_{s}$, a new curve can be obtained from a configuration
$p_{s}^{\prime}$ by rotating and translating the original curve if $p_{s}$ and
$p_{s}^{\prime}$ differ only in position and orientation. However, this is no
longer true if $p_{s}$ and $p_{s}^{\prime}$ differ in higher order states like
curvature. For this reason, position and orientation are called variant states
while higher order states are invariant [10]. When motion primitives are
concatenated, invariant states may experience a discontinuous jump if all
primitives are computed relative to a single starting vertex [10]. Further,
for lattices with non-cardinal headings, using a single starting vertex to
define all motion primitives may result in off-lattice endpoints during
concatenations, requiring rounding and resulting in sub-optimal motions [12,
13]. A solution approach to this critique that we employ is to compute a
control set for a lattice with several starting vertices [10, 5].

####  I-3 Excessive Oscillations

Because the set of available motions is restricted in lattice-based motion
planning, compound motions comprised of several primitives may possess
excessive oscillations [8] resulting in motions which are kinodynamically
feasible but zig-zag unnecessarily.

###  I-A Contributions

The solutions proposed in this paper to the challenges above are summarized
here.

  * •

Choice of control set: We propose a mixed integer linear programming (MILP)
encoding of the MTSCS problem for state lattices with several starting
vertices. This represents the first known non-brute force approach to solving
the MTSCS problem. Though this formulation does not scale for large lattices,
we observe that control sets need only be computed once, offline.

  * •

Continuity in All States: The solution to the proposed MILP is the smallest
control set that generates motions that are continuous in all states with
bounded $t$-factor sub-optimality.

  * •

Excessive Oscillations: We provide a novel algorithm that eliminates redundant
vertices along motions computed using a lattice. This algorithm is based on
shortest paths in directed acyclic graphs and eliminates excessive
oscillations. It runs in time quadratic in the number of motion primitives
along the input motion.

  * •

Planning Algorightm: We present an A*-based algorithm to compute feasible
motions for difficult maneuvers in both parking lot and highway settings. The
algorithm accommodates off-lattice start and goal configurations up to a
specified tolerance.

In our preliminary work [11], we introduced a MILP formulation for the MTSCS
problem for lattices with a single starting vertex and proved the NP-
completeness of this sub-problem. The novel contributions of this paper over
our preliminary work include a MILP formulation of the MTSCS problem for a
general lattice featuring several starting vertices, an A*-based search
algorithm for use in lattice-based motion planning, and a post-processing
smoothing algorithm to remove excessive oscillations from motions planned
using a lattice.

###  I-B Related Work

Sampling based planners work by randomly sampling configurations in a
configuration space and checking connectivity to previous samples. Common
examples include Asymptotically Optimal Rapidly-exploring Random Trees (RRT*)
[14], and Probabilistic Roadmap (PRM) [15]. These methods use a local planner
to expand or re-wire a tree to include new samples. Therefore, many TPBV
problems must be solved online necessitating the use of simplified
kinodynamics. Kinodynamic feasibility may not be guaranteed [4]. Sampling-
based planners, while not complete, can be _asymptotically complete_ with a
convergence rate that worsens as the dimensionality of the problem is
increased [16]. To improve the performance of RRT* in higher dimensions, the
authors of [17] pre-compute a set of reachable configurations from which
random samples are drawn. The pre-computed set is a lattice in which motions
are computed by random sampling. The usefulness of sampling based techniques
is considered to be restricted to unconstrained motion planning problems [18],
limiting their use in scenarios like highway driving.

Techniques using the interpolating curve approach include fitting Bezier
curves, Clothoid curves [19], or polynomial splines [20] to a sequence of way-
points. A typical criticism of clothoid-based interpolation techniques is the
time required to solve TPBV problems involving Fresnel integrals [4, 21].
While TPBV problems are typically easily solved in the case of polynomial
spline and Bezier curve interpolation, it is often difficult to impose
constraints like bounded curvature on these curves owing to their low
malleability [4, 21]. In [22], the authors develop a sampling-based planner
that uses a control-affine dynamic model to facilitate solving TPBV problems.
These motions are then smoothed via numeric optimization in [23]. Here,
motions with non-affine non-holonomic constraints and piece-wise linear
velocity profiles are developed. The authors of [23] illustrate the benefits
of computing way-points using a system of similar complexity to the desired
final motion. However, by the very nature of the simplification, it is
possible for infeasible way-point paths to be developed. A common critique of
both interpolating curve and numerical optimization approaches is their
reliance on global way-points [4, 21]. Moreover, it can be difficult to
specify additional state constraints (e.g., curvature constraints, etc.) [4,
21].

Lattice-based planners are versatile in the problems they address. The authors
of [24], [25] demonstrate lattice adaptations made to account for the
structured environments of urban roads for use in autonomous driving, while in
[26], a set of motion primitives is computed (based on experience) for a UAV
exploring mine-shafts. In [27, 12], the authors use motion primitives to
traverse a lattice with states given by position and heading. In both these
works, the authors use a rounding technique to account for off-lattice
primitive concatenations due to non-cardinal headings. Compound motions
comprised of several primitives are discontinuous in curvature – a known
source of slip and discomfort [28]. To alleviate this curvature discontinuity,
the authors of [29] use the techniques of [27] to compute an initial
trajectory which is then smoothed via numerical optimization. This approach
requires at least as much computation time as the methods proposed in [27]. On
the other hand, the authors of [30] compute motion primitives that minimize
the integral of the squared jerk over the motion but limit their results to
forward motion. In [8] the authors consider the control set to be the entirety
of the lattice. The large control set may necessitate coarser lattices.

The choice of motion primitives is of particular interest. Typically motion
primitives are chosen to achieve certain objectives for the paths they
generate. For example, in [31], the authors introduce the notion of
probabilistic motion primitives which achieve a blending of deterministic
motion primitives to better simulate real user behavior. In [32], Dispertio –
a dispersion minimizing algorithm from [33] – is used to compute a set of
motion primitives that result in motions with minimum dispersion. In [34], a
set of motion primitives is computed that relies on a set of user-specified
maneuvers. In [6], on the other hand, the authors present the notion of using
a MTSCS of motion primitives. These are similar to graph $t$-spanners first
proposed in [35]. The process of computing such a set is elaborated in [36]
where we compute motions between lattice vertices that minimize a user-
specified cost function that is learned from demonstrations. The methods
presented herein may be used in tandem with other work [36, 37] to provide an
improved set of user-specific motion primitives.

In [10], the authors present a heuristic for the MTSCS problem which, though
computationally efficient, does not have any known sub-optimality factor
guarantees. Since a control set may be computed once, offline, and used over
many motion planning problems, the time required to compute this control set
is of less importance than its size.

##  II Lattice Planning & Multi-Start Lattices

In this section, we introduce the notion of a multi-start lattice, and
illustrate its uses in motion planning. We also formulate the Minimum
$t$-Spanning Control Set Problem.

###  II-A Lattice-Based Motion Planning

Let $\mathcal{X}$ denote the configuration space of a vehicle. That is,
$\mathcal{X}$ is a set of tuples — called _configurations_ — whose entries are
the _states_ of the vehicle. A _motion_ from an initial configuration
$p_{s}\in\mathcal{X}$ to a goal $p_{g}\in\mathcal{X}$ is a sequence of
configurations in $\mathcal{X}$ beginning at $p_{s}$ and terminating at
$p_{g}$. Let $\mathcal{M}$ denote the set of all kinodynamically feasible
motions in $\mathcal{X}$ – that is, the motions that adhere to a known
kinodynamic model. We assume that each motion in $\mathcal{M}$ can be
evaluated using a known cost function
$c:\mathcal{M}\rightarrow\mathbb{R}_{\geq 0}$. Though $c$ is left general, we
do assume that it obeys the triangle inequality. The motion planning problem
is as follows.

######  Problem II.1 (Motion planning problem (MPP)).

Given $\mathcal{X}$, a set of obstacles $\mathcal{X}_{\text{obs}}$, start and
goal configurations $p_{s},p_{g}\in\mathcal{X}-\mathcal{X}_{\text{obs}}$, and
a cost function $c$, compute a motion from $p_{s}$ to $p_{g}$ that is feasible
and optimal:

  1. 1.

Feasible motions: Each configuration in the motion is in
$\mathcal{X}-\mathcal{X}_{\text{obs}}$, and the motion is kinodynamically
feasible.

  2. 2.

Optimal motions: The motion minimizes $c$ over all feasible motions from
$p_{s}$ to $p_{g}$.

Lattice-based planning approximates a solution to this problem by regularly
discretizing $\mathcal{X}$ using a _lattice_ , $L\subseteq\mathcal{X}$. Let
$i,j$ be vertices in $L$, and let $p$ be a motion that solves Problem II.1 for
$p_{s}=i,p_{g}=j$, written $i\cdot p=j$. The key observation in lattice-based
motion planning is that $p$ may be applied to other vertices in $L$
potentially resulting in another vertex in $L$. This motivates the use of a
starting vertex $s\in L$, usually at the origin, that represents a
_prototypical_ lattice vertex. Offline, a feasible optimal motion is computed
from $s$ to all other vertices $i\in L-\\{s\\}$ in the absence of obstacles. A
subset $E$ of these motions is selected and used as an action set during an
online search. The motions available at any iteration of the online search are
isometric to those actions available at $s$.

Figure 2: (a) Config. space, lattice and start from (1). (b) Lattice and
$\mathcal{X}$ as in (a) with start set
$\mathcal{S}=[s_{0}=(0,0,0),s_{1}=(0,0,\pi/4)]$.

###  II-B Multi-Start Lattices

There are examples for which a single start is insufficient to capture the
full variety of motions. For example, let

$\begin{split}\mathcal{X}=&\mathbb{R}^{2}\times[0,2\pi],\
L=\mathbb{Z}^{2}\times\\{i\pi/4,i=0,\dots,7\\},\\\ s_{0}=&(0,0,0),\end{split}$
(1)

denote configurations space, lattice, and starting vertex, respectively – as
in Figure 2 (a). If $i=(1,1,\pi/4)\in L$ and $j=(2,2,\pi/4)\in L$, then the
motion $p_{0}$ from $i$ to $j$ is such that $s_{0}\cdot p_{0}\notin L$ – that
is, the motion $p_{0}$ applied to $s_{0}$ is not in the lattice. Therefore,
the simple diagonal motion $p_{0}$ will not appear in any action set $E$ which
may result in motions with excessive oscillations.

We propose a solution to this problem by way of a multi-start lattice. Given a
set of starts $\mathcal{S}\subset L$, vertices $i,j\in L$ and motion $p$ from
$i$ to $j$, we say that $i\cdot p$ is a _valid concatenation_ , and write
$i\oplus p$ if there exists a start $s\in\mathcal{S}$ such that $s\cdot p\in
L$. In the example in Figure 2 (b), let
$\mathcal{S}=[s_{0}=(0,0,0),s_{1}=(0,0,\pi/4)]$, then the motion $p_{0}$ from
$i$ to $j$ can be applied to $s_{1}$ to reach a lattice vertex (namely $i$).
Therefore, $i\oplus p_{0}$ is a valid concatenation.

An ideal set of starts $\mathcal{S}$ would have the property that for every
pair of vertices $i,j\in L$ with motion $p$ from $i$ to $j$, the concatenation
$i\cdot p$ is valid. Obtaining such a set is simple for a class of
configuration spaces described here. For motion planning for autonomous cars
and car-like robots, we typically use a configuration space of the form

$\mathcal{X}=\mathbb{R}^{2}\times[0,2\pi)\times U_{0}\dots\times U_{N},$ (2)

where $(x,y)\in\mathbb{R}^{2}$ represents the planar location of the vehicle,
$\theta\in[0,2\pi)$ the heading, and $U_{i}\subset\mathbb{R}$ higher order
states for $i=0\dots N$. These may include curvature, curvature rate,
velocity, acceleration, and jerk. We make the assumption that each state
$U_{i}$ is bounded by physical constraints of the vehicle, passenger comfort,
or speed limits. This allows us to write
$U_{i}=[U_{i}^{0},U_{i}^{1}]\subset\mathbb{R}$ for some upper and lower limits
$U_{i}^{0},U_{i}^{1}\in\mathbb{R}$. For $\mathcal{X}$ in (2), we construct a
lattice $L\subseteq\mathcal{X}$ by discretizing each state separately. In
particular, for $\alpha,\beta\in\mathbb{R}_{>0}$, and
$n_{0},n_{1},n_{2},m_{i}\in\mathbb{N}$ for $i=0\dots N$, we let

$\begin{split}L=&\\{i\alpha,i=-n_{0}\dots n_{0}\\}\\\
\times&\\{i\beta,i=-n_{1}\dots n_{1}\\}\\\ \times&\\{\pi
i/2^{n_{2}-1},i=0\dots 2^{n_{2}}-1\\}\\\
\times&\prod_{i=0}^{N}\\{U_{i}^{0}+j/m_{i},j=0\dots(U_{i}^{1}-U_{i}^{0})m_{i}\\}.\end{split}$
(3)

This lattice samples $2n_{0}+1$, and $2n_{1}+1$ values of the $x,y$
coordinates, respectively, with spacing $\alpha,\beta$ between samples,
respectively. It partitions heading values $[0,2\pi)$ into $2^{n_{2}}$ evenly
spaced samples, and for $i=0\dots N$, it partitions $U_{i}$ into $m_{i}$
evenly spaced samples. For $\mathcal{X},L$ from (2), (3), let

$\begin{split}\mathcal{S}=\Big{\\{}(&0,0,\theta,u_{0},\dots u_{N})\ :\\\
&\theta\in\\{j\pi/2^{n_{2}-1},j=0,\dots,2^{n_{2}-2}-1\\},\\\
&u_{i}\in\\{U_{i}^{0}+j/m_{i},j=0\dots(U_{i}^{1}-U_{i}^{0})m_{i}\\}\Big{\\}}.\end{split}$
(4)

This starting set has the property that for any vertices $i,j\in L$, the
motion $p$ from $i$ to $j$ is such that $\exists s\in\mathcal{S}$ with
$s\oplus p\in L$.

###  II-C Motion Planning with a Multi-Start Control Set

In this section, we describe how multi-start lattices can be used in motion
planning and we formulate the Minimum $t$-Spanning Control Set (MTSCS)
problem. We assume that for all $i,j\in\mathcal{X}$, there exists a motion $p$
solving Problem II.1 from $i$ to $j$ in the absence of obstacles. We assume
that the cost of this motion, $c(p)$, is known, non-negative, and is equal to
0 if and only if $i=j$. Further, we assume that $c$ obeys the triangle
inequality – that is, if $p_{1}$ is a motion from $i\in\mathcal{X}$ to
$j\in\mathcal{X}$, and $p_{2}$ is a motion from $i$ to $j$ by way of
$r\in\mathcal{X}$, then $c(p_{1})\leq c(p_{2})$. This cost is left general,
and may represent travel time, integral of the squared jerk, etc.

Given $(\mathcal{X},L,\mathcal{S},c)$ of configurations space, lattice,
starting set and cost of motions, respectively, the high level idea behind
motion planning using a multi-start control set is the following. For each
$s\in\mathcal{S}$, we pre-compute a set $\mathcal{B}_{s}$ of cost-minimizing
feasible motions from $s$ to each vertex $i\in L-\mathcal{S}$, and we denote
$\mathcal{B}=\bigcup_{s\in\mathcal{S}}\mathcal{B}_{s}$. We select a subset
$E\subseteq\mathcal{B}$ which we use as an action set during an online search
in the presence of obstacles. To this end, we offer the following definition:

######  Definition II.2 (Relative start).

For vertex $i\in L$, the _relative start_ of $i$ is the vertex
$R(i)\in\mathcal{S}$ such that for each $j\in L$, if $p_{j}$ is the motion
from $R(i)$ to $j$ then $i\oplus p_{j}$ is valid.

To construct a set $E\subseteq\mathcal{B}$, we choose a set $E_{s}$
representing the action set for all lattice vertices with relative start $s$,
and let $E=\bigcup_{s\in\mathcal{S}}E_{s}$. If $L$ and $\mathcal{S}$ are given
by (3), (4), respectively, then the relative start of any configuration
$i=(x,y,\theta,u_{1},\dots,u_{N})\in L$ is
$R(i)=(0,0,\theta^{\prime},u_{1},\dots,u_{N})$ where

$\theta^{\prime}=\frac{\pi}{2^{n_{2}-1}}\left(\left(\frac{2^{n_{2}-1}\theta}{\pi}\right)\mod{4}\right).$

The set of available motions during an online search — i.e., the action set —
at a configuration $i\in L$ is given by $E_{R(i)}$, and the cost of each
motion $p\in E_{R(i)}$ is given by $c(p)$.

For any subset $E\subseteq\mathcal{B}$, we denote by $\bar{E}$ the set of all
tuples $(i,j)\in L^{2}$ such that if $p$ is the optimal feasible motion from
$i$ to $j$, then $p\in E_{R(i)}$:

$\begin{split}\bar{E}&=\\{(i,j)\in L^{2}:(i\oplus p=j)\Rightarrow p\in
E_{R(i)}\\}.\end{split}$ (5)

Computing a motion between lattice vertices given a set $E$ is equivalent to
computing a shortest path in the weighted directed graph
$G=(L,\bar{E}_{\text{CF}},c)$ where $\bar{E}_{\text{CF}}\subseteq\bar{E}$ is
the set of all collision-free edges in $\bar{E}$, and for all
$(i,j)\in\bar{E}_{\text{CF}}$, $c((i,j))$ is the cost of the optimal feasible
motion from $i$ to $j$. This graphical representation of motion planning
motivates the following definitions given the tuple
$(\mathcal{X},L,\mathcal{S},c,E)$:

######  Definition II.3 (Path using $E$).

A _path using_ $E$ from $s\in\mathcal{S}$ to $j\in L$, denoted $\pi^{E}(s,j)$
is the cost-minimizing path from $s$ to $j$ (ties broken arbitrarily) in the
weighted, directed graph $G_{\text{Free}}=(L,\bar{E},c)$ in the absence of
obstacles .

######  Definition II.4 (Motion cost using $E$).

The cost of a motion _using_ $E$ from $s\in\mathcal{S}$ to $j\in L$, denoted
$d^{E}(s,j)$ is the cost of the path $\pi^{E}(s,j)$ in
$G_{\text{Free}}=(L,\bar{E},c)$.

Let $i\in L-\mathcal{S},s\in\mathcal{S}$ and let $p$ a cost minimizing motion
from $s$ to $i$. Observe that if $E=\mathcal{B}$, then $d^{E}(s,j)=c(p)$. This
implies that using $\mathcal{B}$ as a control set will result in paths with
minimal cost. However, because $\mathcal{B}$ can be large, the branching
factor at a vertex $i\in L$ during an online search may be prohibitive. We
therefore wish to limit the size of $|E_{R(i)}|$ while keeping $d^{E}(s,j)$
close to the minimal cost for all $j\in L$. This motivates the following
definition:

######  Definition II.5 ($t$-Error).

Given the tuple $(\mathcal{X},L,\mathcal{S},c,E)$, the $t$-_error_ of $E$ is
defined as

$t\text{Er}(E)=\max_{\begin{subarray}{c}s\in\mathcal{S}\\\ j\in
L-\mathcal{S}\end{subarray}}\ \frac{d^{E}(s,j)}{d^{\mathcal{B}}(s,j)}.$

That is, the $t$-error of a control set $E$ is the worst-case ratio of the
distance using $E$ from a start $s$ to a vertex $j$ to the cost of the optimal
path from $s$ to $j$ taken over all starts $s$ and vertices $j$ in the
lattice. The $t$-error can be used to evaluate the quality of a control set
$E$:

######  Definition II.6 ($t$-Spanning Set).

Given the tuple $(\mathcal{X},L,\mathcal{S},c,E)$, and a real number $t\geq
1$, we say that a set $E$ is a $t$-_spanner_ of $L$ (or $t$-_spans_ $L$), if
$t\emph{Er}(E)\leq t$.

Our objective is to compute a control set $E$ that optimizes a trade-off
between branching factor and motion quality. This is formulated in the
following problem:

######  Problem II.7 (Minimum $t$-spanning Control Set Problem).

Input: A tuple $(\mathcal{X},L,\mathcal{S},c)$, and a real number $t\geq 1$.
Output: A control set $E=\bigcup_{s\in\mathcal{S}}E_{s}$ that $t$-spans $L$
where $\max_{s\in\mathcal{S}}|E_{s}|$ is minimized.

Using a solution $E$ to Problem II.7 as a control set for lattice planning has
two beneficial properties. First, the number of collision-free neighbors of
any vertex $i\in L$ in the graph $G$ is at most $|E_{R(i)}|$ whose maximum
value $\max_{s\in\mathcal{S}}|E_{\mathcal{S}}|$ is minimized. Second, for
every $i\in L-\mathcal{S},s\in\mathcal{S}$, it must hold that $d^{E}(s,i)\leq
td^{\mathcal{B}}(s,i)$. Thus, Problem II.7 represents a trade-off between
branching factor and motion quality. As an alternative, the framework
presented here is capable of computing a $t$-spanning set $E$ such that the
sum $\sum_{s\in\mathcal{S}}|E_{s}|$ is minimized. However, this last does not
ensure a minimum value of the branching factor at each vertex.

##  III Main Results

In this section we present a MILP formulation of Problem II.7, and algorithms
to compute and smooth motions using computed control sets.

###  III-A Multi-Start MTSCS Problem: MILP Formulation

In [38, Theorem 5.2.1], we show that Problem II.7 is NP-hard, which motivates
the MILP formulation presented here. Let $(\mathcal{X},L,\mathcal{S},c)$
denote configuration space, lattice, start set, and cost of vertex-to-vertex
motions in $L$, respectively. For any motion
$q\in\mathcal{B}=\bigcup_{s\in\mathcal{S}}\mathcal{B}_{s}$, let

$S_{q}=\\{(i,j)\ :\ i,j\in L,\ i\oplus q=j\\}.$

Thus $S_{q}$ is the set of all pairs $(i,j)\in L^{2}$ such that $q$ is a cost-
minimizing feasible motion from $i$ to $j$ and $i\cdot q$ is valid. By
definition of valid concatenations, there exists $s\in\mathcal{S}$ and
$j^{\prime}\in L$ such that $q$ is a cost-minimizing feasible motion from $s$
to $j^{\prime}$ (i.e., $s=R(i)$) implying that $(s,j^{\prime})\in S_{q}$.

Let $E=\bigcup_{s\in\mathcal{S}}E_{s}$ be a solution to Problem II.7. Let
$G_{\text{Free}}=(L,\bar{E},c)$ be the weighted directed graph with edges
$\bar{E}$ given in (5). For each $s\in\mathcal{S}$ and each $(i,j)\in\bar{E}$,
we make a copy $(i,j)^{s}$ of the edge. This allows us to treat edges
differently depending on the starting vertex of the path to which they belong.
For each $r\in L$, a path using $E$ from $s$ to $r$, $\pi^{E}(s,r)$, may be
expressed as a sequence of edges $(i,j)^{s}$ where $(i,j)\in\bar{E}$. For each
$s\in\mathcal{S}$ we construct a new graph

$T^{s}=(L,E_{T}^{s}),$ (6)

whose edges $E_{T}^{s}$ are defined as follows: let

$\mathcal{P}^{s}=\bigcup_{i\in L-\mathcal{S}}\pi^{E}(s,i).$

Thus $\mathcal{P}^{s}$ is the set of all minimal cost paths from $s$ to
vertices $i\in L-\mathcal{S}$ in the graph $G_{\text{Free}}$. These paths are
expressed as a sequence of edge copies $(i,j)^{s}$ where $(i,j)\in\bar{E}$.
For each $i\in L-\mathcal{S}$, and for each $s\in\mathcal{S}$, if
$\mathcal{P}^{s}$ contains two paths $\pi^{E}_{1},\pi^{E}_{2}$ from $s$ to
$i$, determine the last common vertex $j$ in paths $\pi^{E}_{1},\pi^{E}_{2}$,
and delete the the copy of the edge in $\pi^{E}_{2}$ whose endpoint is $j$
from $\mathcal{P}^{s}$. The remaining edges are the set $E_{T}^{s}$. The graph
$T^{s}$ has a useful property defined here:

######  Definition III.1 (Arborescence).

From Theorem 2.5 of [39], a graph $T$ with a vertex $s$ is an arborescence
rooted at $s$ if every vertex in $T$ is reachable from $s$, but deleting any
edge in $T$ destroys this property.

Intuitively, if $T$ is an arborescence rooted at $s$, then for each vertex
$j\neq s$ in $T$, there is a unique path in $T$ from $s$ to $j$. For each
$s\in\mathcal{S}$, the graph $T^{s}$ an arborescence rooted at $s$. This is
shown in the following Lemma:

######  Lemma III.2 (Arborescence Lemma).

Let $E=\bigcup_{s\in\mathcal{S}}E_{s}$ be a solution to Problem II.7, and
$G_{\text{Free}}=(L,\bar{E},c)$. For each $s\in\mathcal{S}$, $T^{s}$ (in (6))
is an arborescence rooted at $s$. Further, $\forall i\in L-\mathcal{S}$,
$d^{E}(s,i)$ is the length of the path in $T^{s}$ to $i$.

###### Proof.

Let $s\in\mathcal{S}$. To show that $T^{s}$ is an arborescence rooted at $s$,
observe that there is a path in $T^{s}$ from $s$ to all $i\in L$. Indeed, if
$E$ solves Problem II.7, then $E$ $t$-spans $L$. In particular, there must be
at least one path, $\pi^{E}(s,i)$ using $E$ from $s$ to each $i\in L$ implying
that $\pi^{E}(s,i)\in\mathcal{P}^{s}$. Since $E_{T}^{s}$ deletes duplicate
paths from $\mathcal{P}^{s}$, there must be a path in $T^{s}$ from $s$ to $i$.
By construction of the edge set $E_{T}^{s}$, duplicate paths (with equal cost)
are deleted ensuring the uniqueness of paths from $s$ to each $i\in L$
completing the proof that $T^{s}$ is an arborescence rooted at $s$. The cost
of the path in $T^{s}$ from $s$ to $i$ is defined as the cost of the path
$\pi^{E}(s,i)$ which is $d^{E}(s,i)$. ∎

Lemma III.2 implies that $E$ is a $t$-spanner of $L$ if and only if $\forall
s\in\mathcal{S}$ there is a corresponding arborescence $T^{s}$ rooted at $s$
whose vertices are $L$, whose edges $(i,j)^{s}$ are copies of members of
$S_{q}$ for some $q\in E$, and such that the cost of the path from $s$ to any
vertex $i\in L$ in $T^{s}$ is no more than a factor of $t$ from the optimal
path from $s$ to $i$. Indeed, the forward implication follows from Lemma
III.2, while the converse holds by definition of a $t$-spanner. From this, we
develop four criteria that represent necessary and sufficient conditions for
$E$ to be a $t$-spanning control set of $L$:

Usable Edge Criteria:

For any $q\in\mathcal{B}$, for any $s\in\mathcal{S}$, and for any $(i,j)\in
S_{q}$, the copy $(i,j)^{s}$ may belong to a path in $T^{s}$ from $s$ to a
vertex $r\in L$ if and only if $q\in E_{R(i)}$.

Cost Continuity Criteria:

For any $s\in\mathcal{S}$, $q\in\mathcal{B}$, $(i,j)\in S_{q}$, and
$(i,j)^{s}$ a copy of $(i,j)$, if $(i,j)^{s}$ lies in the path in $T^{s}$ to
vertex $j$ then $c(\pi^{E}(s,j))=c(\pi^{E}(s,i))+c(q)$. That is, the cost of
the path from $s$ to $j$ in $T^{s}$ is equal to the cost of the path from $s$
to $i$ plus the cost of the motion from $i$ to $j$.

$t$-Spanning Criteria:

For any vertex $j\in L-\mathcal{S}$, and for any start $s\in\mathcal{S}$, the
cost of the path in $T^{s}$ from $s$ to $j$ can be no more than $t$ times the
cost of the direct motion from $s$ to $j$.

Arborescence Criteria:

The set $T^{s}$ must be an arborescence for all $s\in\mathcal{S}$.

We now present a MILP encoding of these criteria. Let $|L|=n$ and all the
vertices are enumerated as $1,2,...,n$ with $s\in\mathcal{S}$ taking the
values $1,\dots,m$ for $m\leq n$. For any control set
$E=\bigcup_{s\in\mathcal{S}}E_{s}$, define $m(n-m-1)$ decision variables:

$y_{q}^{s}=\begin{cases}1,\ \text{if }q\in E_{s}\\\ 0,\
\text{otherwise}.\end{cases},\ q=m+1,\dots,n$

For each edge $(i,j)\in L^{2}$, and each $s\in\mathcal{S}$ let

$x_{ij}^{s}=\begin{cases}1\ &\text{if }(i,j)^{s}\in T^{s}\\\ 0\
&\text{otherwise}.\end{cases}$

That is, $x_{ij}^{s}=1$ if $(i,j)^{s}$ (the copy of the edge $(i,j)$ for start
$s\in\mathcal{S}$) lies on a path from $s$ to a vertex in the lattice. Let
$z_{i}^{s}$ denote the length of the path in the tree $T^{s}$ to vertex $i$
for any $i\in L$, $c_{ij}$ the cost of the optimal feasible motion from $i$ to
$j$, and let $L^{\prime}=L-\mathcal{S}$. The criteria above can be encoded as
the following MILP:

$\displaystyle\min_{K\in\mathbb{R}}\ K$ (7a) $\displaystyle s.t.\ \forall
s\in\mathcal{S}$ $\displaystyle\sum_{q\in L^{\prime}}y_{q}^{s}\leq K,$ (7b)
$\displaystyle x_{ij}^{s^{\prime}}-y_{q}^{s}\leq 0,\ \ $
$\displaystyle\forall(i,j)\in S_{q},$ (7c) $\displaystyle\forall
q\in\mathcal{B},\ \forall s^{\prime}\in\mathcal{S},$ $\displaystyle
z_{i}^{s}+c_{ij}-z_{j}^{s}\leq M_{ij}^{s}(1-x_{ij}^{s}),\ $
$\displaystyle\forall(i,j)\in L\times L^{\prime}$ (7d) $\displaystyle
z_{j}^{s}\leq tc_{sj},\ $ $\displaystyle\forall j\in L^{\prime}$ (7e)
$\displaystyle\sum_{i\in L}x_{ij}^{s}=1,\ $ $\displaystyle\forall j\in
L^{\prime}$ (7f) $\displaystyle x_{ij}^{s}\in\\{0,1\\},\ $
$\displaystyle\forall(i,j)\in L\times L^{\prime}$ (7g) $\displaystyle
y_{q}^{s}\in\\{0,1\\},\ $ $\displaystyle\forall q\in\mathcal{B},$ (7h)

where $M_{ij}^{s}=tc_{si}+c_{ij}-c_{sj}$. The objective function (7a) together
with constraint (7b) minimizes $\max_{s\in\mathcal{S}}|E_{s}|$ as in Problem
II.7. The remainder of the constraints encode the four criteria guaranteeing
that $E$ is a $t$-spanning set of $L$:

Constraint (7c): Let $q$ be the motion in $\mathcal{B}$ from $s\in\mathcal{S}$
to $j\in L-\mathcal{S}$. If $q\not\in E_{s}$, then $y_{q}^{s}=0$ by
definition. Therefore, (7c) requires that $x_{ij}^{s^{\prime}}=0$ for all
$(i,j)\in S_{q},s^{\prime}\in\mathcal{S}$. Alternatively, if $y_{q}^{s}=1$,
then $x_{ij}^{s^{\prime}}$ is free to take values $1$ or $0$ for any $(i,j)\in
S_{q},s^{\prime}\in\mathcal{S}$. Thus constraint (7c) encodes the Usable Edge
Criteria.

Constraint (7d): Constraint (7d) takes a similar form to [40, Equation
(3.7a)]. Note that $M^{s}_{ij}\geq 0$ for all
$(i,j)\in\mathcal{B},s\in\mathcal{S}$. Indeed, $\forall t\geq 1$,
$M_{ij}^{s}\geq c_{si}+c_{ij}-c_{sj}$, and $c_{si}+c_{ij}\geq c_{sj}$ by the
triangle inequality. Replacing $M_{ij}^{s}$ in (7d) yields

$z_{i}^{s}+c_{ij}-z_{j}^{s}\leq(tc_{si}+c_{ij}-c_{sj})(1-x_{ij}).$ (8)

If $x_{ij}^{s}=1$, then $(i,j)^{s}$ is on the path in $T^{s}$ to vertex $j$
and (8) reduces to $z_{j}^{s}\geq z_{i}^{s}+c_{ij}$ which encodes the Cost
Continuity Criteria. If, however, $x_{ij}^{s}=0$, then (8) reduces to
$z_{i}^{s}-z_{j}^{s}\leq tc_{si}-c_{sj}$ which holds trivially by constraint
(7e) and by noting that $z_{j}^{s}\geq c_{sj},\forall j\in L$ by the triangle
inequality.

Constraint (7f): Constraint (7f) together with constraint (7d) yield the
Arborescence Criteria. Indeed for all $s\in\mathcal{S}$, by Theorem 2.5 of
[39], $T^{s}$ is an arborescence rooted at $s$ if every vertex in $T^{s}$
other than $s$ has exactly one incoming edge, and $T^{s}$ contains no cycles.
The constraint (7f) ensures that every vertex in $L^{\prime}$, which is the
set of all vertices in $T^{s}$ other than those in $\mathcal{S}$, have exactly
one incoming edge, while constraint (7d) ensures that $T^{s}$ has no cycles.
Indeed, suppose that a cycle existed in $T^{s}$, and that this cycle contained
vertex $i\in L^{\prime}$. Suppose that this cycle is represented as

$i\rightarrow j\rightarrow\dots\rightarrow k\rightarrow i.$

Recall that it is assumed that the cost of any motion between two different
configurations in $\mathcal{X}$ is strictly positive. Therefore, (7d) implies
that $z_{i}^{s}<z_{j}^{s}$ for any $(i,j)\in L^{2}$. Therefore,

$z_{i}<z_{j}<\dots<z_{k}<z_{i},$

which is a contradiction.

Figure 3: Example motion planning using PrAC for a 2-start lattice.

###  III-B Motion Planning With a MTSCS

The previous section illustrates how to compute a control set
$E=\bigcup_{s\in\mathcal{S}}E_{s}$ given the tuple
$(\mathcal{X},L,\mathcal{S},c)$. We have also presented a description of how
such a control set may be used during an online A*-style search of the graph
$G=(L,\bar{E}_{\text{CF}},c)$ from a start vertex $p_{s}\in L$ to a goal
vertex $p_{g}\in L$. Here, the edge set $\bar{E}_{\text{CF}}$ is the set of
pairs $(i,j)\in L^{s}$ such that there exists a collision-free motion $p\in
E_{R(i)}$ from $i$ to $j$ where $R(i)\in\mathcal{S}$ is the relative start of
$i$. The motion primitives in this control set could be used in any graph-
search algorithm. In this section, we propose one such algorithm: an A*
variant, _Primitive A* Connect_ (PrAC) for path computation in $G$. The
algorithm follows the standard A* algorithm closely with some variations
described here:

Weighted fScore: In the standard A* algorithm, two costs are maintained for
each vertex $i$: the _gScore_ , $g(i)$ representing the current minimal cost
to reach vertex $i$ from the starting vertex $p_{s}$, and the fScore,
$f(i)=g(i)+h(i)$ which is the sum of the gScore and an estimated cost to reach
the goal vertex $p_{g}$ by passing through $i$ given heuristic $h$. We
implement the weighted fScore from [41]: Given a value $\lambda\in[0,1]$, we
let $a=0.5\lambda,b=1-0.5\lambda$, and we define a new cost function
$f^{\prime}=ag(i)+bh(i)$. If $\lambda=1$, then both gScore and heuristic are
weighted equally and standard A* is recovered. However, as $\lambda$
approaches 0, more weight is placed on the heuristic which promotes
_exploration_ over _optimization_. While using a value $\lambda<1$ eliminates
optimality guarantees, it also empirically improves runtime performance. In
practice, using $\lambda=1$ works well for maneuvers where $p_{s}$ and $p_{g}$
are close together, like parallel parking, while small values of $\lambda$
work well for longer maneuvers like traversing a parking lot.

Expanding Start and Goal Vertices: We employ the bi-directional algorithm from
[42] with the addition of a direct motion. In detail, we expand vertices
neighboring both start and goal vertices and attempt to connect these vertices
on each iteration. In essence, we double the expansion routine at each
iteration of A* once from $p_{s}$ to $p_{g}$ and once from $p_{g}$ to $p_{s}$
with reverse orientation. We maintain two trees, one rooted at the start
vertex $p_{s}$, denoted $T_{s}$, the other at the goal, $T_{g}$ whose leaves
represent open sets $O_{s},O_{g}$, respectively. We also maintain two sets
containing the current best costs $g(p_{s},i)$ to get from $p_{s}$ to each
vertex $i\in T_{s}$, and from $p_{g}$ to each vertex $j\in T_{g}$ (traversed
in reverse), $g(p_{g},j)$, respectively. Given an admissible heuristic
$h^{\prime}$, we define a new heuristic $h$ as

$\begin{split}h(i)=\min_{j\in O_{g}}h^{\prime}(i,j)+g(p_{g},j),\ \forall i\in
O_{s}\\\ h(j)=\min_{i\in O_{s}}h^{\prime}(j,i)+g(p_{s},i),\ \forall j\in
O_{g}\end{split}$ (9)

On each iteration of the A* while loop, let $i\in O_{s}$ be the vertex that is
to be expanded, let $j\in O_{g}$ be the vertex that solves (9) for $i$, and
let $p$ denote the pre-computed cost-minimizing feasible motion from $i$ to
$j$. We expand vertex $i$ by applying available motions in
$E_{R(i)}\cup\\{p\\}$ where $E_{R(i)}$ is the set of available motions at the
relative start of $i$. The addition of $p$ to the available motions improves
the performance of the algorithm by allowing quick connections between
$T_{s},T_{g}$ where possible. In the same iteration, we then swap $T_{s}$ and
$T_{g}$ and perform the same steps but with all available motions in reverse.
This is illustrated in Figure 3. Expanding start and goal vertices has proven
especially useful during complex maneuvers like backing into a parking space.

Off-lattice Start and Goal Vertices: For a configuration $v\in\mathcal{X}$ and
a lattice $L$ given by (3), let $\text{Round}(v)$ denote a function that
returns the element of $L$ computed by rounding each state of $v$ to the
closest discretized value of that state in $L$. For lattice $L$ with start set
$S$ given by (4) and control set $E$ obtained by solving the MILP in (7), it
is likely that the start and goal configurations $p_{s},p_{g}\in\mathcal{X}$
do not lie in $L$. This is particularly true in problems that necessitate
frequent re-planning. While increasing the fidelity of the lattice or
computing motions online between lattice vertices and $p_{s},p_{g}$ (e.g.,
[8]) can address this issue, both these methods adversely affect the
performance of the planner. Instead we propose a method using lattices with
graduated fidelity, a concept introduced in [7]. We compute a control set
$E_{\text{off}}$ of primitives from off-lattice configurations lattice
vertices. These can be traversed in reverse to bring lattice vertices to off-
lattice configurations. The technique is summarized in Algorithm 1 which is
executed offline. The algorithm takes as input $L,E$, and a vector of
tolerances for each state
$Tol=(x_{\text{tol}},y_{\text{tol}},\theta_{\text{tol}},u_{0,\text{tol}},\dots,u_{N,\text{tol}})$.
It first computes a set $Q\subset\mathcal{X}$ such that for every
configuration in $v\in\mathcal{X}$ there exists a configuration $q\in Q$ that
can be translated to $q^{\prime}\in\mathcal{X}$ where each state of
$q^{\prime}$ is within the accepted tolerance of $v$ (Line 2). For each
element of $Q$, we determine the lattice vertex $q^{\prime}=\text{Round}(q)$,
and the set of available actions at the relative start of $q^{\prime}$,
$E_{R(q^{\prime})}$ . For each primitive in $E_{R(q^{\prime})}$, we compute a
motion from $q$ to a lattice vertex close to the endpoint of $p$ and store it
in a set $E_{q}$ (Lines 7-13). In Lines 9, 10 the vertex $p$ is modified to
$p^{\prime}$ that is no closer to $q$ than $p$. This is to ensure that the
motion added to $E_{q}$ in Line 13 does not posses large loops not present the
primitive $p$. These loops can arise if the start and end configurations of a
motion are too close together. An illustrative example of this principle is
given in Figure 4.

Figure 4: Algorithm 1 Lines 9-13 Example. (a) Configuration
$q\in\mathcal{X}-L$, lattice vertex $q^{\prime}=\text{Round(q)}$, and
primitive $p\in E_{R(q^{\prime})}$. (b) Motion from $q$ to end of $p$ results
in a loop as $q,p$ are too close together. (c) Vertex $p$ replaced with
neighboring vertex $p^{\prime}$ and a motion from $q$ to $p^{\prime}$ is
computed.

######  Theorem III.3 (Completeness).

If $\lambda=1$ and $p_{s},p_{g}\in L$, then PrAC returns the cost-minimizing
path in $G=(L,\bar{E}_{\text{CF}},c)$.

###### Proof.

Given the assumptions of the Theorem, we may view PrAC as standard A* where
the expansion of $T_{g}$ serves only to update the heuristic. That is, PrAC
reduces to standard A* with a heuristic given by (9) for all $i\in O_{s}$. At
each iteration of A*, $h$ is improved by expanding $T_{g}$. By the
completeness of A* for admissible heuristics and finite branching factor
(which is the case here for finite control sets), it suffices to show that $h$
is indeed admissible. At each iteration of A*, let $i$ be the vertex in
$O_{s}$ that is to be expanded. Let $\pi$ be the optimal path from $i$ to
$p_{g}$ in $G$. Then $\pi$ must pass through a vertex $r$ in $O_{g}$. This
result holds by the completeness of A* with initial configuration $p_{g}$ with
paths traversed in reverse. Then by the triangle inequality, $c(\pi)\geq
c(\pi^{\bar{E}_{\text{CF}}}(i,r))+g(p_{g},r)$. Finally, because $h^{\prime}$
is an admissible heuristic, $c(\pi^{\bar{E}_{\text{CF}}}(i,r))+g(p_{g},r)\geq
h^{\prime}(i,r)+g(p_{g},r)\geq\min_{j\in
O_{g}}h^{\prime}(i,j)+g(p_{g},j)=h(i)$. Therefore, $h(i)\leq c(\pi)$ which
concludes the proof. ∎

Algorithm 1 Generate Off-Lattice Control Set

1:procedure OffLattice($L,E,Tol$)

2: $E_{\text{off}}\leftarrow\emptyset$
$\begin{split}Q\leftarrow&\\{2ix_{\text{tol}},i=0,\dots,\nicefrac{{\alpha}}{{2x_{\text{tol}}}}\\}\\\
&\times\\{2iy_{\text{tol}},i=0,\dots,\nicefrac{{\beta}}{{2y_{\text{tol}}}}\
\\}\\\
&\times\\{2i\theta_{\text{tol}},i=0,\dots,\nicefrac{{(\pi-\theta_{\text{tol}})}}{{\theta_{\text{tol}}}}\\}\\\
&\times\prod_{i=0}^{N}\\{U_{i}^{0}+2ju_{i,\text{tol}},j=0,\dots\nicefrac{{(U_{i}^{1}-U_{i}^{0})}}{{2u_{i\text{tol}}}}\\}.\end{split}$

3: for $q\in Q$ do

4: $(x_{q},y_{q},\theta_{q},u_{0,q},\dots,u_{N,q})\leftarrow q$

5: $E_{q}\leftarrow\emptyset$

6: $q^{\prime}\leftarrow\text{Round}(q)$

7: for $p\in E_{R(q^{\prime})}$ do

8: $(x_{p},y_{p},\theta_{p},u_{0,p},\dots,u_{N,p})\leftarrow p$

9: for $x\in\\{x_{p},x_{p}+\text{sign}(x_{p}-x_{q})\alpha\\}$ do

10: for $y\in\\{y_{p},y_{p}+\text{sign}(y_{p}-y_{q})\beta\\}$ do

11: $p^{\prime}\leftarrow(x,y,\theta_{p},u_{0,p},\dots,u_{N,p})$

12: Compute motion from $q$ to $q^{\prime}$

13: Add motion of lowest cost over all $p^{\prime}$ to $E_{q}$

14: Add $E_{q}$ to $E_{\text{off}}$

15: return $E_{\text{off}}$

###  III-C Motion Smoothing

Given the tuple $(\mathcal{X},L,\mathcal{S},c,E)$, of configuration space,
lattice, cost function, and MTSCS, respectively, let
$G=(L,\bar{E}_{\text{CF}},c)$ be the weighted directed graph with edge set
$\bar{E}_{\text{CF}}$ is the collision-free subset of $\bar{E}$ given in (5).

We now present a smoothing algorithm based on the _shortcut_ approach that
takes as input a path in $G$, here $\pi^{E}(p_{s},p_{g})$, between start and
goal configurations $p_{s},p_{g}$. This path is expressed as a sequence of
edges in $\bar{E}_{\text{CF}}$. Thus,
$\pi^{E}(p_{s},p_{g})=\\{(i_{r},i_{r+1}),r=1,\dots,m-1\\}$ for some
$m\in\mathbb{N}_{\geq 2}$ where $(i_{r},i_{r+1})\in\bar{E}_{\text{CF}}$ for
all $r=1,\dots,m-1$ and $i_{1}=p_{s},i_{m}=p_{g}$. Let $C_{\pi}$ denote the
set of all configurations along motions from $i_{r}$ to $i_{r+1}$ for all
$(i_{r},i_{r+1})\in\pi^{E}(p_{s},p_{g})$. Algorithm 2 summarizes the proposed
approach.

Algorithm 2 Smoothing Lattice Motion

1:procedure
DagSmooth($\pi^{E}(p_{s},p_{g}),C_{\pi},\mathcal{X}_{\text{obs}},c,n,\Psi$)

2: $V_{1}\leftarrow$SampleRandom$(n,C_{\pi})$

3: $V\leftarrow\\{i_{r}\\}_{r=1}^{m}\cup V_{1}$ in the order that they appear
in $C_{\pi}$

4: $\\{i_{r}\\}_{r=1}^{m+n}\leftarrow V$

5: for $i\in V$ do

6: $\text{dist}(i)=\infty$

7: $\text{dist}(i_{1})=0$

8: $\text{Pred}(i_{1})=$None

9: for $u$ from 1 to $n+m-1$ do

10: for $v$ from $u+1$ to $m+n$ do

11: $p_{1}=$ motion from $i_{u}$ to $i_{v}$

12: $p_{2}=$ motion from $i_{v}$ to $i_{u}$

13: $\underline{c}=\min\big{(}c(p_{1}),\Psi(c(p_{2}))\big{)}$,

14: $\underline{p}=\text{arg}\min\big{(}c(p_{1}),\Psi(c(p_{2}))\big{)}$

15: $\overline{c}=\max\big{(}c(p_{1}),\Psi(c(p_{2}))\big{)}$,

16: $\overline{p}=\text{arg}\max\big{(}c(p_{1}),\Psi(c(p_{2}))\big{)}$

17: if $\text{dist}(i_{u})+\underline{c}\leq\text{dist}(i_{v})$ then

18: if CollisionFree$(\underline{p},\mathcal{X}_{\text{obs}})$ then

19: $\text{dist}(i_{v})=\text{dist}(i_{u})+\underline{c}$

20: $\text{Pred}(i_{v})=i_{u}$

21: else

22: if $\text{dist}(i_{u})+\overline{c}\leq\text{dist}(i_{v})$ then

23: if CollisionFree$(\overline{p},\mathcal{X}_{\text{obs}})$ then

24: $\text{dist}(i_{v})=\text{dist}(i_{u})+\overline{c}$

25: $\text{Pred}(i_{v})=i_{u}$ return Backwards chain of predecessors from
$i_{m}$

Algorithm 2 takes as input a collision-free, dynamically feasible path
$\pi^{E}$ between start and goal configurations – such as that returned by
PrAC – as well as the set of all configurations $C_{\pi}$, obstacles
$\mathcal{X}_{\text{obs}}$, cost function of motions $c$, and a non-negative
natural number $n$. It also takes a function
$\Psi:\mathbb{R}\rightarrow\mathbb{R}$ which is used to penalize reverse
motion. That is, given two configurations $i,j\in\mathcal{X}$ with a motion
$p$ from $i$ to $j$, we say that the cost of the motion $p$ is $c(p)$, while
the cost of the reverse motion $p^{\prime}$ from $j$ to $i$ that is identical
to $p$ but traversed backwards is $c(p^{\prime})=\Psi(c(p))$.

The set $V$ in Line 3 represents sampled configurations along the set of
configurations $C_{\pi}$ which includes all endpoints of the edges
$(i_{r},i_{r+1})\in\pi^{E}$, as well as $n$ additional random configurations
along motions connecting these endpoints. Configurations in $V$ must be in the
order in which they appear along $\pi^{E}$. For each pair ($i_{u},i_{v}$) of
configurations with $i_{v}$ appearing after $i_{u}$ in $\pi^{E}$, Algorithm 2
attempts to connect $i_{u}$ to $i_{v}$ with either a motion from $i_{u}$ to
$i_{v}$ (Line 11) or from $i_{v}$ to $i_{u}$ traversed backwards, selecting
the cheaper of the two if possible (Lines 17-25).

Were we to form a graph with vertices $V$ and with edges $(i_{u},i_{v})\in
V^{2}$ where $i_{v}$ occurs farther along $C_{\pi}$ than $i_{u}$ and where the
optimal feasible motion from $i_{u}$ to $i_{v}$ is collision-free, then
observe that this graph would be a directed, acyclic graph (DAG). Indeed, were
this graph not acyclic, then the path $\pi^{E}(p_{s},p_{g})$ would contain a
cycle implying that a configuration would appear at least twice in
$\pi^{E}(p_{s},p_{g})$. This is impossible by construction of the PrAC
algorithm which maintains two trees rooted at $p_{s},p_{g}$, respectively.
This motivates the following observation and Theorem:

######  Observation III.4.

The nested for loop in lines 9-10 of Algorithm 2 constructs a DAG and finds
the minimum-cost path from $p_{s}$ to $p_{g}$ in the graph.

######  Theorem III.5.

Let $\pi_{1}^{E}$ be the input path to Algorithm 2 between configurations
$p_{s}$, $p_{g}$ with cost $c(\pi^{E}_{1})$. Let $\pi^{E}_{2}$ be the path
returned by Algorithm 2 with cost $c(\pi^{E}_{2})$. Then $c(\pi^{E}_{2})\leq
c(\pi^{E}_{1})$. Moreover, if $n=0$, this algorithm runs in time quadratic in
the number of vertices along $\pi^{E}_{1}$.

###### Proof.

By Observation III.4, Algorithm 2 constructs a DAG containing configurations
$p_{s},p_{g}$ as vertices. It also solves the minimum cost path problem on
this DAG. Observe further that by construction of the DAG vertex set in Line
3, all configurations along the original path $\pi^{E}_{1}$ are in $V$. Thus
$\pi^{E}_{1}$ is an available solution to the minimum path problem on the DAG.
This proves that the minimum cost path can do no worse than $c(\pi^{E}_{1})$.
If $n=0$, then $V$ is the set of endpoints of edges in $\pi^{E}_{1}$.
Therefore, $V\subseteq L$. Because all motions between lattice vertices have
been pre-computed in $\mathcal{B}$, Lines 11,12 can be executed in constant
time, and the nested for loop in lines 9-10 will thus run in time $O(m)^{2}$
where $m$ is the number of vertices on the path $\pi^{E}_{1}$. ∎

Figure 5: Comparison of smoothing algorithms for same input path (red). Blue:
path smoothed using Algorithm 2. Red: path smoothed using Algorithm 1 from [8]
(no change from input path).

Algorithm 2 is similar to Algorithm 1 in [8] with one critical difference. The
latter algorithm adopts a greedy approach, connecting the start vertex $i_{1}$
in the path $\pi^{E}$ to the farthest vertex $i_{k}\in\pi^{E}$ that can be
reached without collisions. This process is then repeated at vertex $i_{k}$
until the goal vertex is reached. Though this greedy approach also runs in
time quadratic in the number of vertices along the input path, the cost of
paths returned will be no lower than the cost of paths returned by Algorithm 2
proposed herein. A simple example is illustrated in Figure 5. Using the
control set in Figure 5, an initial path is computed from $i_{1}=p_{s}$ to
$i_{6}=p_{g}$ (red). This path remains unchanged when smoothed using the
proposed Algorithm in [8]. However, Algorithm 2 will return the less costly
blue path.

##  IV Results

We verify our proposed technique against two techniques in two common
navigational settings: Hybrid A* [27], and CG [30]. The techniques described
here were implemented in Python 3.7 (Spyder). Results were obtained using a
desktop equipped with an AMD Ryzen 3 2200G processor and 8GB of RAM running
Windows 10 OS. Start and goal configurations were not constrained to be
lattice vertices. We assume that all obstacles are known to the planner ahead
of time, and that the environment is noiseless. As such, the results that
follow can be thought of as a single iteration of a full re-planning process.

###  IV-A Memory

The motions used in this section are $G3$ curves which take the form CCC, or
CSC where “C” denotes a curved segment, and “S” a straight segment. Such
motions can be easily generated from 9 constants in the case of motion
planning without velocity, and 19 constants otherwise [43]. Thus instead of
storing all configurations along a motion, we store only these constants.

###  IV-B Parking Lot Navigation

We begin by validating the proposed method against a common technique: Hybrid
A* [27] in a parking lot scenario. Though Hybrid A* is not a new algorithm,
more recent state of the art approaches use Hybrid A* to plan an initial
motion which is then refined (e.g. [29]). We are therefore motivated to
compare the run-time and path quality of the approach proposed in this paper
to Hybrid A* whose run-time is a lower bound of all state of the art
algorithms using it as a sub-routine. In [43], we present a method of
computing motions between start and goal configurations in the configurations
space $\mathcal{X}=\mathbb{R}^{2}\times[0,2\pi)\times\mathbb{R}^{3}$. Here,
configurations take the form $(x,y,\theta,\kappa,\sigma,\rho)$ where
$(x,y,\theta)$ represent the planar coordinates and heading of a vehicle,
$\kappa$ the curvature, $\sigma$ the curvature rate (defined as $d\kappa/ds$
for arc-length $s$), and $\rho$ the second derivative of curvature with
respect to arc-length. We generate motions with continuously differentiable
curvature profiles assuming that $\kappa,\sigma,\rho$ are bounded in
magnitude. The motivation for this work comes from the observation that jerk,
the derivative of acceleration with respect to time, is a known source of
discomfort for the passengers of a car [28]. In particular, minimizing the
integral of the squared jerk is often used a cost function in autonomous
driving [30]. Since this value varies with $\sigma$, keeping $\sigma$ low and
bounded is desirable in motion planning for autonomous vehicles.

Unfortunately, due to the increased complexity of the configuration space
over, say, the configuration space used in the development of Dubins’ paths,
$\mathcal{X}_{\text{Dubins}}=\mathbb{R}^{2}\times[0,2\pi)$, solving TPBV
problems in $\mathcal{X}$ takes on average two orders of magnitude more time
than computing a Dubins’ path. Though motions computed using the techniques in
[44], [43] are more comfortable, and result in lower tracking error than
Dubins’ paths, they may be computationally impractical to use in motion
planners that require solving many TPBV problems online. However, they prove
to be particularly useful in the development of pre-computed motion
primitives.

####  IV-B1 Lattice Setup & Pruning

The configuration space used here is
$\mathcal{X}=\mathbb{R}^{2}\times[0,2\pi)\times\mathbb{R}^{3}$ with
configurations $(x,y,\theta,\kappa,\sigma,\rho)$. Motion primitives were
generated using the MILP in (7) for a $15\times 20$ square lattice with 16
headings and 3 curvatures, and a value of $t=1.1$ (10% error from optimal). To
account for the off-lattice start-goal pairs, we used a higher-fidelity
lattice with 64 headings and 30 curvatures. Lattice vertex values of
$\sigma,\rho$ were set to 0. This results in a start set $\mathcal{S}$ with 12
starts given by (4). The cost $c$ of a motion is defined by the arc-length of
that motion. These motions were computed using our work in [43] with bounds on
$\kappa,\sigma,\rho$:

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

which were deemed comfortable for a user [44], particularly at low speeds
typical of parking lots. The spacing of the lattice $x,y$-values was chosen to
be $r_{\text{min}}/4$ for a minimum turning radius
$r_{\text{min}}=1/\kappa_{\text{max}}$. Finally, if the cost of the motion
from $s\in\mathcal{S}$ to a vertex $j$ was larger than 1.2 times the Euclidean
distance from $s$ to $j$ for all $s\in\mathcal{S}$, then $j$ was removed from
the lattice. This technique which we dub _lattice pruning_ is to keep the
lattice relatively small, and to remove vertices for which the optimal motion
requires a large loop. The value of 1.2 comes from the observation that the
optimal motion from the start vertex $s=(0,0,0,\kappa_{\text{max}})$ to
$j=(r_{\text{min}},r_{\text{min}},\pi/2,\kappa_{\text{max}})$ is a quarter
circular arc of radius $r_{\text{min}}$. The ratio of the arc-length of this
maneuver to the Euclidean distance from $s$ to $j$ is $\pi/(2\sqrt{2})\approx
1.11$. Thus using a cutoff value of 1.2 admits a sharp left and right quarter
turn but is still relatively small.

####  IV-B2 Adding Reverse Motion

The motion primitives returned by the MILP in (7) are motions between a
starting vertex $s\in\mathcal{S}$, and a lattice vertex $j\in L-\mathcal{S}$.
As such, they are for forward motion only. To add reverse motion primitives to
the control set $E_{s}$ for each $s\in\mathcal{S}$, we applied the forward
primitives to $\mathcal{S}$ in reverse. We then rounded the final
configurations of these primitives to the closest lattice vertex. For each
$(x,y)$-value of the final configurations, we select a single configuration
$(x,y,\theta,\kappa)$ that minimizes arc-length. This is to keep the branching
factor of an online search low. Finally, to each
$s=(0,0,\theta,\kappa)\in\mathcal{S}$ we add three primitives:
$(0,0,\theta,\pm\kappa_{\text{max}}),(0,0,\theta,0)$ with a reverse motion
penalty. These primitives reflect the cars ability to stop and instantaneously
change its curvature.

####  IV-B3 Scenario Results

We verified our results in five parking lot scenarios (a)-(e). The first four
scenarios illustrate our technique in parking lots requiring forward and
reverse parking. The results are illustrated in Figure 6. Here, we have
compared out approach to Hybrid A* using an identical collision checking
algorithm, and using the same heuristic (that proposed in [27]). Though the
motions may appear similar, they are actually quite different. This difference
is illustrated in Figure 7 which shows the heading $\theta$ of the vehicle
along the motion proposed herein vs that of Hybrid A*. Heading profiles for
the other scenarios are not included for brevity, though results are similar.

Figure 6: Scenarios (a) - (d). Red paths from proposed method, yellow from
Hybrid A*. Figure 7: Heading along Motion for Scenario (a). Orange: Hybrid A*
motion, Blue: proposed.

To evaluate the quality of the motions predicted, we use three metrics: the
integral of the square jerk (IS Jerk), final arc-length, and runtime. These
three metrics are expressed as ratios of the value obtained using the methods
of [27] to those of the proposed. The results are summarized in Table I.
Observe that because these techniques are determinisitc, no standard
deviations are presented. The major difference between the two approaches can
be seen in the IS Jerk Reduction. That is, the ratio of the IS Jerk using the
methods of [27] to those of the proposed. This is because the motion
primitives we employ are each $G3$ curves with curvature rates bounded by what
is known to be comfortable. Observe that the value of IS Jerk obtained using
our approach is up to 16 times less than that of [44]. In fact, using a Hybrid
A* approach may result in motions with infeasibly large curvature rates
resulting in larger tracking errors and increased danger to pedestrians.

Despite the bounds on curvature rate, the final arc-lengths of curves computed
using our approach are comparable to those of Hybrid A*. Furhter, though
Dubins’ paths (which are employed by Hybrid A*) take on average two orders of
magnitude less time to compute than $G3$ curves, the runtime performance of
our method often exceeds that of Hybrid A*. In fact, our proposed method
takes, on average 6.9 times less time to return a path, exceeding the average
runtime speedup of the method proposed in [30]. Moreover, the methods in [30]
do not account for reverse motion, and also assume that a set of way-points
between start and goal configurations is known.

The only scenario in which Hybrid A* produces a motion in less time than the
proposed method is Scenario (c) in which Hybrid A* produced a path with no
reverse motion (which accounts for the speedup). However, in order to produce
this motion, the curvature of the motion must change instantaneously multiple
times resulting in an IS Jerk that is 16.3 times higher than the proposed
method. The low run-time of the proposed method may due to the length of
primitives we employ. It has been observed that Hybrid A* often takes several
iterations to obtain a motion of comparable length to one of our primitives.
This results in a much larger open set during each iteration of A*.

The final scenario we investigated is a parallel parking scenario (scenario
(e)) which is illustrated in Figure 8. Though the motion computed with Hybrid
A* may appear simpler, it requires a curvature rate that is 16.7 times larger
than what is considered comfortable. It should also be noted that several
other parallel parking scenarios in which the clearance between obstacles was
decreased were considered. While the proposed method returned a path in each
of these scenarios, Hybrid A* failed to produce a path in the allotted time.

Figure 8: Scenario (e). Scenario |  IS Jerk Reduction |  Length Reduction |  Runtime Speedup
---|---|---|---
$(a)$ |  7.7 |  1.0 |  5.3
$(b)$ |  9.9 |  1.0 |  18.4
$(c)$ |  10.1 |  1.0 |  0.6
$(d)$ |  16.3 |  2.2 |  7.3
$(e)$ |  16.7 |  0.6 |  3.0
TABLE I: Scenario Results

A complex parking lot navigation scenario using the motion primitives from
this section can be found in Figure 1.

###  IV-C Speed Lattice

In this experiment, we generate a full trajectory (including both path and
speed profile) for use in highway driving using our approach. Here, we use
only forward motion as reverse motion on a highway is unlikely.

In addition to developing $G3$ paths, our work in [43] also details a method
with which a trajectory with configurations
$(x,y,\theta,\kappa,\sigma,\rho,v,a,\beta)$ may be computed. Here, $v,a,\beta$
represent velocity and longitudinal acceleration, and longitudinal jerk
respectively. The approach is to compute profiles of $\rho$ and $\beta$ that
result in a trajectory that minimizes a user-specified cost function. This
cost function is a weighted sum of undesirable trajectory features including
the integral of the square (IS) acceleration, IS jerk, IS curvature, and final
arc-length. The key feature of this approach is that both path (tuned by
$\rho$) and velocity profile (tuned by $\beta$) are optimized simultaneously,
keeping path planning in-loop during the optimization. As in the previous set
of examples, computing trajectories via the methods outlined in [43] require
orders of magnitude more time than simple Dubins’ paths.

However, pre-computing a set of motion primitives where each motion is itself
computed using the methods of [43] ensures that every motion used in PrAC is
optimal for the user. Moreover, because we include velocity in our
configurations – and therefore in our primitives – we do not need to compute a
velocity profile.

####  IV-C1 Lattice Setup & Pruning

Motion primitives were generated (7) for a $24\times 32$ grid. Dynamic bounds
for comfort were kept at (10). The $x$ component of the lattice vertices were
sampled every $r_{\text{min}}/6$ meters while the $y$ components were sampled
every $r_{\text{min}}/12$. Headings were sampled every $\pi/16$ radians (32
samples). We also assumed values of $\kappa=\sigma=\alpha=0$ on lattice
vertices. To account for off-lattice start-goal pairs, we use a higher-
fidelity lattice with 128 headings, and 10 curvatures between
$-\kappa_{\text{max}},\kappa_{\text{max}}$. Finally, five evenly spaced
velocities were sampled between 15 and 20 km/hr. A value of $n=0$ was used in
the smoothing Algorithm 2.

####  IV-C2 Scenario Results

The highway scenario was chosen to closely resemble the roadway driving
scenario in [30]. The results of this sceario can be found in Figure 9, while
performance analysis is summarized in Table II. The metrics used to measure
performance are the arc-length of the proposed motion, the smoothness cost of
the motion, the maximum curvature obtained over the motion, and a runtime
speedup normalized to Hybrid A* (HA*). The final column of the Table indicates
weather a velocity profile was included during the motion computation. In
Table II, two values of smoothness cost are given in the form of a tuple
$(\text{Smoothness}_{1},\text{Smoothness}_{2})$ where

$\begin{split}\text{Smoothness}_{1}&=\sum_{i=1}^{N-1}|\Delta\mathbf{x}_{i+1}-\Delta\mathbf{x}_{i}|^{2},\\\
\text{Smoothness}_{2}&=\int_{0}^{s_{f}}\kappa(s)^{2}ds.\end{split}$

The first measure is used in [27], where $N$ configurations are sampled along
a motion, with $\textbf{x}_{i}$ denoting the vector of $x,y$-components of the
$i^{th}$ configuration, and
$\Delta\textbf{x}_{i}=\textbf{x}_{i}-\textbf{x}_{i-1}$. The second measure is
used in [30]. The first two methods appearing in the Table II are computed
directly from the motions in Figure 9, while the second two come from [30] for
an identical experiment. Here, CG refers to the method proposed in [30], while
HA2* refers to the implementation of Hybrid A* as it appears in [30].

The authors of [30] report an average runtime speedup of 4.5 times as compared
to Hybrid A* for the path planning phase (without speed profile). On average,
PrAC computed a full motion, including a speed profile 4.7 times faster than
the time required for Hybrid A* to compute a path. Furthermore, the use of
PrAC with $G3$ motion primitives significantly reduced the smoothness cost in
both of its possible definitions.

Figure 9: Result of highway maneuver with several obstacles.  Method |  Length ($m$) |  Smoothness Cost |  Max Curvature ($m^{-1}$) |  Runtime Speedup |  Velocity
---|---|---|---|---|---
HA* |  65.0 |  (1.35, 0.77) |  0.198 |  1 |  No
Proposed |  64.6 |  (0.17, 0.28) |  0.158 |  4.7 |  Yes
CG |  65.8 |  ( - , 0.44) |  0.189 |  4.5 |  No
HA2* |  65.6 |  ( - , 0.88) |  0.196 |  1 |  No
TABLE II: Road navigation results: HA* and proposed shown above. CG and HA2*
from [30] Table 3 for similar motion planning problem.

##  V Discussion & Conclusion

In this work, we present a novel technique to compute an optimal set of motion
primitives for use in lattice-based motion planning by way of a mixed integer
linear program. We also present an A*-based algorithm which uses these
primitives to compute motions between configurations. Finally, we develop a
post-processing smoothing algorithm to remove excessive oscillations from the
resulting motions.

The results of the previous section illustrate the effectiveness of the
proposed technique. Indeed, feasible, smooth motions were computed between
start and goal locations in both parking lot and highway settings. By adding
reverse motion primitives, complex problems like navigating an obstacle-rich
parking lot, or parallel parking were solved. Moreover, if the motion
primitives already include velocity as a state, then a velocity profile may be
easily computed. This paper has not proposed a controller to track the
reference paths we compute nor does it propose a framework for re-planning.
These are left for future work.

## References

  * [1] B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” _IEEE Transactions on Intelligent Vehicles_ , vol. 1, no. 1, pp. 33–55, 2016\.
  * [2] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer _et al._ , “Autonomous driving in urban environments: Boss and the urban challenge,” _Journal of Field Robotics_ , vol. 25, no. 8, pp. 425–466, 2008.
  * [3] O. Andersson, O. Ljungqvist, M. Tiger, D. Axehill, and F. Heintz, “Receding-horizon lattice-based motion planning with dynamic obstacle avoidance,” in _2018 IEEE CDC_ , pp. 4467–4474.
  * [4] D. González, J. Pérez, V. Milanés, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 17, no. 4, pp. 1135–1145, 2015.
  * [5] M. Tiger, D. Bergström, A. Norrstig, and F. Heintz, “Enhancing lattice-based motion planning with introspective learning and reasoning,” _IEEE RA-L_ , vol. 6, no. 3, pp. 4385–4392, 2021.
  * [6] M. Pivtoraiko and A. Kelly, “Generating near minimal spanning control sets for constrained motion planning in discrete state spaces,” in _2005 IEEE/RSJ IROS_ , pp. 3231–3237.
  * [7] M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” _Journal of Field Robotics_ , vol. 26, no. 3, pp. 308–333, 2009.
  * [8] R. Oliveira, M. Cirillo, B. Wahlberg _et al._ , “Combining lattice-based planning and path optimization in autonomous heavy duty vehicle applications,” in _2018 IEEE Intelligent Vehicles Symposium_ , pp. 2090–2097.
  * [9] L. Janson, B. Ichter, and M. Pavone, “Deterministic sampling-based motion planning: Optimality, complexity, and performance,” _The International Journal of Robotics Research_ , vol. 37, no. 1, pp. 46–61, 2018.
  * [10] M. Pivtoraiko and A. Kelly, “Kinodynamic motion planning with state lattice motion primitives,” in _2011 IEEE/RSJ IROS_ , pp. 2172–2179.
  * [11] A. Botros and S. L. Smith, “Computing a minimal set of t-spanning motion primitives for lattice planners,” in _2019 IEEE/RSJ IROS_ , pp. 2328–2335.
  * [12] A. Rusu, S. Moreno, Y. Watanabe, M. Rognant, and M. Devy, “State lattice generation and nonholonomic path planning for a planetary exploration rover,” in _65th International Astronautical Congress 2014 (IAC)_ , vol. 2, p. 953.
  * [13] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semi-structured environments,” _The International Journal of Robotics Research_ , vol. 29, no. 5, pp. 485–501, 2010\.
  * [14] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The International Journal of Robotics Research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [15] L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE Transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, 1996.
  * [16] K. Solovey, L. Janson, E. Schmerling, E. Frazzoli, and M. Pavone, “Revisiting the asymptotic optimality of RRT,” in _2020 IEEE ICRA_ , pp. 2189–2195.
  * [17] S. D. Pendleton, W. Liu, H. Andersen, Y. H. Eng, E. Frazzoli, D. Rus, and M. H. Ang, “Numerical approach to reachability-guided sampling-based motion planning under differential constraints,” _IEEE RA-L_ , vol. 2, no. 3, pp. 1232–1239, 2017.
  * [18] L. Claussmann, M. Revilloud, D. Gruyer, and S. Glaser, “A review of motion planning for highway autonomous driving,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 21, no. 5, pp. 1826–1848, 2019.
  * [19] H. Fuji, J. Xiang, Y. Tazaki, B. Levedahl, and T. Suzuki, “Trajectory planning for automated parking using multi-resolution state roadmap considering non-holonomic constraints,” in _2014 IEEE Intelligent Vehicles Symposium (IV)_ , pp. 407–413.
  * [20] C. G. L. Bianco and A. Piazzi, “Optimal trajectory planning with quintic g2-splines,” in _2000 IEEE Intelligent Vehicles Symposium (IV)_ , 2000, pp. 620–625.
  * [21] A. Ravankar, A. A. Ravankar, Y. Kobayashi, Y. Hoshino, and C.-C. Peng, “Path smoothing techniques in robot navigation: State-of-the-art, current and future challenges,” _Sensors_ , vol. 18, no. 9, p. 3170, 2018.
  * [22] E. Schmerling, L. Janson, and M. Pavone, “Optimal sampling-based motion planning under differential constraints: the driftless case,” in _2015 IEEE ICRA_ , pp. 2368–2375.
  * [23] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach to smooth trajectories for motion planning with car-like robots,” in _2015 IEEE CDC_ , pp. 835–842.
  * [24] M. Rufli and R. Siegwart, “On the design of deformable input-/state-lattice graphs,” in _2010 IEEE ICRA_ , pp. 3071–3077.
  * [25] M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in _2011 IEEE ICRA_ , pp. 4889–4895.
  * [26] M. Dharmadhikari, T. Dang, L. Solanka, J. Loje, H. Nguyen, N. Khedekar, and K. Alexis, “Motion primitives-based path planning for fast and agile exploration using aerial robots,” in _2020 IEEE ICRA_ , pp. 179–185.
  * [27] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Practical search techniques in path planning for autonomous driving,” _Ann Arbor_ , vol. 1001, no. 48105, pp. 18–80, 2008.
  * [28] J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt _et al._ , “Towards fully autonomous driving: Systems and algorithms,” in _2011 IEEE Intelligent Vehicles Symposium (IV)_ , pp. 163–168.
  * [29] X. Zhang, A. Liniger, and F. Borrelli, “Optimization-based collision avoidance,” _IEEE Transactions on Control Systems Technology_ , vol. 29, no. 3, pp. 972–983, 2021.
  * [30] Y. Zhang, H. Chen, S. L. Waslander, J. Gong, G. Xiong, T. Yang, and K. Liu, “Hybrid trajectory planning for autonomous driving in highly constrained environments,” _IEEE Access_ , vol. 6, pp. 32 800–32 819, 2018.
  * [31] A. Paraschos, C. Daniel, J. R. Peters, and G. Neumann, “Probabilistic movement primitives,” _Advances in neural information processing systems_ , vol. 26, pp. 2616–2624, 2013.
  * [32] L. Jarin-Lipschitz, J. Paulos, R. Bjorkman, and V. Kumar, “Dispersion-minimizing motion primitives for search-based motion planning,” _arXiv preprint arXiv:2103.14603_ , 2021.
  * [33] L. Palmieri, L. Bruns, M. Meurer, and K. O. Arras, “Dispertio: Optimal sampling for safe deterministic motion planning,” _IEEE RA-L_ , vol. 5, no. 2, pp. 362–368, 2019.
  * [34] K. Bergman, O. Ljungqvist, and D. Axehill, “Improved optimization of motion primitives for motion planning in state lattices,” in _2019 IEEE Intelligent Vehicles Symposium (IV)_ , pp. 2307–2314.
  * [35] D. Peleg and A. A. Schäffer, “Graph spanners,” _Journal of graph theory_ , vol. 13, no. 1, pp. 99–116, 1989.
  * [36] A. Botros, N. Wilde, and S. L. Smith, “Learning control sets for lattice planners from user preferences,” in _WAFR_. Springer, 2020, pp. 381–397.
  * [37] R. De Iaco, S. L. Smith, and K. Czarnecki, “Learning a lattice planner control set for autonomous vehicles,” in _2019 IEEE Intelligent Vehicles Symposium (IV)_ , pp. 549–556.
  * [38] A. Botros, “Lattice-based motion planning with optimal motion primitives,” 2021\.
  * [39] B. Korte, J. Vygen, B. Korte, and J. Vygen, _Combinatorial optimization_ , 6th ed. Springer, 2018.
  * [40] J. Desrosiers, Y. Dumas, M. M. Solomon, and F. Soumis, “Time constrained routing and scheduling,” _Handbooks in operations research and management science_ , vol. 8, pp. 35–139, 1995.
  * [41] I. Pohl, “Heuristic search viewed as path finding in a graph,” _Artificial intelligence_ , vol. 1, no. 3-4, pp. 193–204, 1970.
  * [42] J. Chen, R. C. Holte, S. Zilles, and N. R. Sturtevant, “Front-to-end bidirectional heuristic search with near-optimal node expansions,” _arXiv preprint arXiv:1703.03868_ , 2017.
  * [43] A. Botros and S. L. Smith, “Tunable trajectory planner using g3 curves,” _IEEE Transactions on Intelligent Vehicles_ , 2022.
  * [44] H. Banzhaf, N. Berinpanathan, D. Nienhüser, and J. M. Zöllner, “From $G2$ to $G3$ continuity: Continuous curvature rate steering functions for sampling-based nonholonomic motion planning,” in _2018 IEEE Intelligent Vehicles Symposium (IV)_ , pp. 326–333.

|  Alexander Botros is a postdoctoral fellow in the Autonomous Systems Lab at
the University of Waterloo. His research focuses primarily on local planners
and trajectory generation for autonomous vehicles. In particular, Alex is
researching the problem of computing minimal t-spanning sets of edges for
state lattices with the goal of using these sets as motion primitives for
autonomous vehicles. Alex Completed his undergraduate and M.Sc. engineering
work at Concordia University in Montreal, and his Ph.D. at the University of
Waterloo.
---|---
|  Stephen L. Smith (S’05–M’09–SM’15) received the B.Sc. degree from Queen’s
University, Canada in 2003, the M.A.Sc. degree from the University of Toronto,
Canada in 2005, and the Ph.D. degree from UC Santa Barbara, USA in 2009. From
2009 to 2011 he was a Postdoctoral Associate in the Computer Science and
Artificial Intelligence Laboratory at MIT, USA. He is currently an Associate
Professor in Electrical and Computer Engineering at the University of
Waterloo, Canada and a Canada Research Chair in Autonomous Systems. His main
research interests lie in control and optimization for autonomous systems,
with an emphasis on robotic motion planning and coordination.
---|---