# Bi-Level Optimization Augmented with Conditional Variational Autoencoder for
Autonomous Driving in Dense Traffic

Arun Kumar Singh1 Jatan Shrestha1 Nicola Albarella2 1University of Tartu,
Estonia 2University of Naples Federico II, ItalyAll authors contributed
equally; ordering is purely alphabetical. Code is available on the project
website: https://sites.google.com/view/mpc-bi-level

###### Abstract

Autonomous driving has a natural bi-level structure. The goal of the upper
behavioural layer is to provide appropriate lane change, speeding up, and
braking decisions to optimize a given driving task. However, this layer can
only indirectly influence the driving efficiency through the lower-level
trajectory planner, which takes in the behavioural inputs to produce motion
commands. Existing sampling-based approaches do not fully exploit the strong
coupling between the behavioural and planning layer. On the other hand, end-
to-end Reinforcement Learning (RL) can learn a behavioural layer while
incorporating feedback from the lower-level planner. However, purely data-
driven approaches often fail in safety metrics in unseen environments. This
paper presents a novel alternative; a parameterized bi-level optimization that
jointly computes the optimal behavioural decisions and the resulting
downstream trajectory. Our approach runs in real-time using a custom GPU-
accelerated batch optimizer, and a Conditional Variational Autoencoder learnt
warm-start strategy. Extensive simulations show that our approach outperforms
state-of-the-art model predictive control and RL approaches in terms of
collision rate while being competitive in driving efficiency.

##  I Introduction

Motion planning for autonomous driving can be divided into two hierarchical
components. At the top level, the behavioural layer computes decisions such as
lane change, speeding up and braking based on the traffic scenario and the
driving task. The behavioural inputs can be conveniently parameterized as set-
points for longitudinal velocity, lateral offsets from the centre line, goal
positions, etc. The advantage of such representation is that it naturally
integrates with the lower-level optimal trajectory planner [1], [2], [3], [4]
[5], [6]. The behavioural layer can be critical for driving in dense traffic
as it can guide the lower-level planner into favourable state-space regions,
much in the same way a collision-free global plan can make the task of the
local planner easier.

#### Existing Gaps

The aim of the behavioural layer is to come up with correct lane change,
acceleration and braking decisions to optimize a given driving task. However,
it can only indirectly affect the driving efficiency through the lower-level
trajectory planner. Clearly, there is strong inter-dependency between the two
layers. However, existing trajectory sampling approaches [4] [5] do not fully
exploit this inter-dependency (see Fig. 1(a) and Section IV). For example,
these works have no mechanism for modifying the behavioural inputs based on
how the associated lower-level trajectory performs on the given driving task.
Reinforcement learning (RL) techniques address this drawback by learning the
behavioural layer. The rewards from the environment act as a feedback to
modify the behavioural inputs while taking into account the effect of the
lower-level planner [1], [2], [3], [4]. Though effective, especially in sparse
traffic scenarios, the purely data-driven approaches typically struggle with
safety metrics in unseen environments.

In this paper, we present a novel approach that estimates the direction in
which the behavioural inputs need to be perturbed in order to improve the
optimality of the lower-level trajectory with respect to the driving task
(Fig.1 (a)). Our core contributions are:

Figure 1: Fig.(a): The solid blocks represent the components of the pipeline
presented in works like [4] [5], while the blocks with dotted boundaries
represent our contribution. Existing works draw behavioural inputs
$\textbf{p}_{j}$ from a distribution and solve a simple QP trajectory planner
for all those inputs. However, there is no mechanism to modify the behavioural
input sampling based on the performance of the lower-level planner on the
driving task. We address this issue by adding a gradient estimation block and
a projection operator to aid constraint satisfaction. Fig.(b) Our bi-level
optimizer ensures safe driving in dense and potentially rash traffic.

  * •

We propose a bi-level optimization where the upper-level variables represent
the behavioural inputs while that at the lower level represent the associated
motion plans.

  * •

We combine Quadratic Programming (QP) with gradient-free optimization for
solving the bi-level problem. Our approach avoids the common pitfalls of the
Gradient descent-based approaches for bi-level optimization (see Section IV).

  * •

We train a Conditional Variational Autoencoder with a differentiable
optimization layer-based network architecture for warm-starting our bi-level
optimizer.

  * •

Our approach outperforms state-of-the-art Model Predictive Control (MPC) and
RL-based approaches in safety metrics in dense traffic.

Figure 2: Fig.(a),(b): A common parametrization for behavioural inputs is in
the form of set-points for lateral offset (Fig. (a) (top)) [1], [2]. It can be
used to induce lane-change manoeuvres in the ego vehicle. As shown in Fig.
(a)(bottom), for long horizon planning, we can split the trajectory segments
into multiple parts and assign a lateral offset set-point to each of them.
Fig.(c) shows different trajectories generated by sampling lateral offsets
$y_{d,m}$ and forward velocity $v_{d,m}$ set-points from a Gaussian
distribution and using them in (3a)-(3d). Fig.(d) showcases our custom batch
optimizer that can solve the lower-level trajectory planning for all the
sampled behavioural inputs in parallel. The figure represents the distribution
of trajectories obtained in a cluttered environment, with a blue rectangle
representing static parked vehicles.

##  II Mathematical Preliminaries

#### Symbols and Notation

Normal font lower-case letters will represent scalars, and bold font variants
will represent vectors. The upper-case bold font letters will represent
matrices. The superscript $T$ will denote the transpose of a matrix or a
vector.

###  II-A Trajectory Parametrization

Using the differential flatness of the bi-cycle car model, we aim to directly
plan in the positional space ($x(t),y(t)$) of the ego-vehicle. Thus, we
parametrize the position-level trajectory of the ego-vehicle in terms of
polynomials in the following form:

$\displaystyle\begin{bmatrix}x(t_{1}),\dots,x(t_{m})\end{bmatrix}=\textbf{W}\textbf{c}_{x},\begin{bmatrix}y(t_{1}),\dots,y(t_{m})\end{bmatrix}=\textbf{W}\textbf{c}_{y},$
(1)

where, W is a matrix formed with time-dependent polynomial basis functions and
$\textbf{c}_{x},\textbf{c}_{y}$ are the coefficients of the polynomial. We can
also express the derivatives in terms of $\dot{\textbf{W}},\ddot{\textbf{W}}$.

###  II-B Behavioral Input Parametrization

We consider the following list of behavioural inputs in our formulation.

  * •

The planning horizon is split into $m$ parts and we assign a desired lateral
offset set-point ($y_{d,m}$) to each of these segments (see Fig.2).Similarly,
we assign longitudinal velocity set-points ($v_{d,m}$) to each of the $m$
segments

  * •

Lastly, works like [7], [8] also include goal positions along longitudinal
($x_{f}$) and lateral directions ($y_{f}$) as behavioural inputs.

We stack all the behavioural inputs into one parameter vector:

$\displaystyle\textbf{p}=\begin{bmatrix}y_{d,1},y_{d,2},\dots,v_{d,1},v_{2,2},\dots
v_{d,m},x_{f},y_{f}\end{bmatrix}.$ (2)

Note that not all elements of p need to be used simultaneously in the lower-
level trajectory planner. For example, [1], [2] use a single set-point for
lateral offset and desired velocity as behavioural inputs. Our simulation
results shown in Section V used four set-points for lateral offset and desired
velocities to construct p. Nevertheless, (2) represents all the behavioural
parameterization that can be accommodated within our bi-level optimizer
discussed in the next section.

###  II-C Lower-Level Trajectory Planning

The lower-level trajectory optimization is formulated in the Frenet-frame:
longitudinal ($x(t)$) and lateral ($y(t)$) motions of the ego-vehicle occur
along and orthogonal to the reference centre line, respectively.

$\displaystyle\min\sum_{t}c_{s}(x(t),y(t))+c_{l}(x(t),y(t))+c_{v}(x(t),y(t))$
(3a)
$\displaystyle(x(t_{0}),y(t_{0}),\dot{x}(t_{0}),\dot{y}(t_{0}),\ddot{x}(t_{0}),\ddot{y}(t_{0}))=\textbf{b}_{0}.$
(3b) $\displaystyle(x(t_{f}),y(t_{f}),\dot{y}(t_{f}))=(x_{f},y_{f},0).$ (3c)
$\displaystyle g_{j}(x(t),y(t))\leq 0,\forall j,t$ (3d) $\displaystyle
c_{s}(x(t),y(t))=(\ddot{x}(t))^{2}+\ddot{y}(t)^{2}$ (4a) $\displaystyle
c_{l}(x(t),y(t))=\ddot{y}(t)-k_{p}(y(t)-\textbf{y}_{d})-k_{v}\dot{y}(t))^{2}$
(4b) $\displaystyle
c_{v}(x(t),y(t))=(\ddot{x}(t)-k_{p}(\dot{x}(t)-\textbf{v}_{d})$ (4c)

The first term ($c_{s}(.)$) in the cost function (3a) ensures smoothness in
the planned trajectory by penalizing high accelerations. The last two terms
($c_{o}(.),c_{v}(.)$) model the tracking of lateral offset ($y_{d,m}$) and
forward velocity $(v_{d,m})$ set-points respectively and is inspired from
works like [1]. For the former, we define a Proportional Derivative (PD) like
tracking with gain $(k_{p},k_{v})$. For the velocity part, we only use a
proportional term. The vector $\textbf{y}_{d},\textbf{v}_{d}$ is formed by
repeating each $y_{d,m},v_{d,m}$ appropriate times and then vertically
stacking them. Equality constraints (3b) ensures that the planned trajectory
satisfies the initial boundary conditions. Thus, vector $\textbf{b}_{0}$ is
simply a stacking of initial position, velocity, and acceleration. The final
boundary conditions are represented through constraints (3c). Inequalities
(3d) enforces collision avoidance, velocity, acceleration, centripetal
acceleration, curvature bounds, and lane boundary constraints. We present the
exact algebraic form of these constraints in Appendix D.

##  III Main Results

###  III-A Proposed Bi-Level Optimization

We formulate combined behavior and trajectory planning through the following
bi-level optimization problem.

$\displaystyle\min_{\textbf{p}}c_{u}(\boldsymbol{\xi}^{*}(\textbf{p}),$ (5a)
$\displaystyle\boldsymbol{\xi}^{*}\in\operatorname*{arg\,min}_{\boldsymbol{\xi}}\frac{1}{2}\boldsymbol{\xi}^{T}\textbf{Q}\boldsymbol{\xi}+\textbf{q}^{T}(\textbf{p})\boldsymbol{\xi}_{j},$
(5b)
$\displaystyle\textbf{A}_{eq}\boldsymbol{\xi}=\textbf{b}(\textbf{p}),\qquad\textbf{g}(\boldsymbol{\xi})\leq\textbf{0}$
(5c)

where (5b)-5c is the matrix representation of (3a)-(3d) obtained using (1).
The behavioural inputs p are defined in (2). Thus a part of it which comprises
lateral offsets and forward velocity set-points enters the cost while the goal
positions enter the affine equality constraints. The variable of the lower-
level problem is $\boldsymbol{\xi}=(\textbf{c}_{x},\textbf{c}_{y})$.

As shown, we have an upper-level cost $c_{u}(.)$ that models the driving task.
It depends on the optimal solution $\boldsymbol{\xi}^{*}$ computed from the
lower-level trajectory optimization. The lower-level optimization explicitly
depends on the parameter p while the upper-level has an implicit dependency
through $\boldsymbol{\xi}^{*}(\textbf{p})$. The goal of the lower-level
optimizer is to compute an optimal solution for a given p. The upper level, in
turn, aims to modify the parameter itself to drive down the upper-level cost
associated with the optimal solution.

###  III-B Batch Optimization and Sampling-based Gradient for Bi-level
Optimization

Our main idea is to apply a gradient-free optimization technique on the upper-
level cost. Alg.1 summarizes the main steps of our proposed bi-level
optimizer, wherein the left superscript $l$ is used to track variables across
iterations. For example, ${{}^{l}}\boldsymbol{\mu}_{p}$ represents the mean of
the sampling distribution at iteration $l$. On line 4, we draw $\overline{n}$
samples of $\textbf{p}_{j}$ from a Gaussian distribution with mean
${{}^{l}\boldsymbol{\mu}_{p}}$ and covariance
${{}^{l}\boldsymbol{\Sigma}_{p}}$. On line 6, we solve the lower-level
trajectory optimization for each sampled parameter. Here, we adopt a two-step
approach, a sample output of which is shown in Fig.2(c). In the first step, we
solve the trajectory optimization without the inequality constraints, and
then, in the second step, we project the obtained solution to the constrained
set. On line 9, we compute the constraint residuals resulting from the optimal
solutions. In line 10, we select the top $n$ samples with the least constraint
residual to create $ConstraintEliteSet$. Line 11 constructs an augmented cost
obtained by evaluating the upper-level cost $c_{u}(\boldsymbol{\xi}^{*}_{j})$
on the samples from the $ConstraintEliteSet$ and adding the corresponding
constraint residuals to it. On line 13, we select the top $q$ samples with the
least augmented cost to construct the $EliteSet$. On line 14, we update the
mean and variance of the sampling distribution of p based on the samples of
the $EliteSet$.

####  III-B1 Updating the Sampling Distribution

There are several ways to update the mean and variance on line 14 of Algorithm
1. The simplest among these is to just fit a Gaussian distribution to the
samples of $\boldsymbol{\xi}_{j}^{*}$ belonging to the $EliteSet$. However,
this approach ignores the exact cost associated with the samples. Thus, in
this work, we use the following update rule from sampling-based optimization
proposed in [9].

$\displaystyle{{}^{l+1}}\boldsymbol{\mu}_{p}=(1-\eta){{}^{l}}\boldsymbol{\mu}_{p}+\eta\frac{\sum_{j=1}^{j=q}s_{j}\textbf{p}_{j}}{\sum_{j=1}^{j=q}s_{j}},$
(6a)
$\displaystyle{{}^{l+1}}\boldsymbol{\Sigma}_{p}=(1-\eta){{}^{l}}\boldsymbol{\Sigma}_{p}+\eta\frac{\sum_{j=1}^{j=q}s_{j}(\textbf{p}_{j}-{{}^{l+1}}\boldsymbol{\mu}_{p})(\textbf{p}_{j}-{{}^{l+1}}\boldsymbol{\mu}_{p})^{T}}{\sum_{j=1}^{j=q}s_{j}}$
(6b) $\displaystyle
s_{j}=\exp{\frac{-1}{\gamma}(c_{u}(\boldsymbol{\xi}_{j}^{*})+r_{j}(\boldsymbol{\xi}^{*}_{j})}$
(6c)

where $\eta$ is the learning-rate and $\gamma$ is some scaling constant. As
discussed in [9], the update rules (6a) and (6b) are obtained by
exponentiating the cost and then performing a sample estimate of its gradient
with respect to the sampled argument (in this case p).

1 $N$ = Maximum number of iterations

2 Initiate mean ${}^{l}\boldsymbol{\mu}_{p},^{l}\boldsymbol{\Sigma}_{p}$, at
$l=0$

3 for  _$l=1,l\leq N,l++$_ do

4 Draw $\overline{n}$ Samples
$(\textbf{p}_{1},\textbf{p}_{2},\textbf{p}_{j},....,\textbf{p}_{\overline{n}})$
from $\mathcal{N}(^{l}\boldsymbol{\mu}_{p},^{l}\boldsymbol{\Sigma}_{p})$

5 Initialize $CostList$ = []

6

7 Solve the lower-level trajectory optimization $\forall\textbf{p}_{j}$:

8 • Step 1: Solve the QP without inequalities
$\displaystyle\overline{\boldsymbol{{\xi}}}_{j}=\arg\min_{\boldsymbol{\xi}_{j}}\frac{1}{2}\boldsymbol{\xi}_{j}^{T}\textbf{Q}\boldsymbol{\xi}_{j}+\textbf{q}^{T}(\textbf{p}_{j})\boldsymbol{\xi}_{j}$
$\displaystyle\textbf{A}_{eq}\boldsymbol{\xi}_{j}=\textbf{b}(\textbf{p}_{j})$
• Step 2: Project to Constrained Set

9
$\displaystyle\boldsymbol{{\xi}}^{*}_{j}=\arg\min_{\boldsymbol{\xi}_{j}}\frac{1}{2}\|\boldsymbol{\xi}_{j}-\overline{\boldsymbol{\xi}}_{j}\|_{2}^{2}$
$\displaystyle\textbf{A}_{eq}\boldsymbol{\xi}_{j}=\textbf{b}(\textbf{p}_{j}),\qquad\textbf{g}(\boldsymbol{\xi}_{j})\leq\textbf{0}$

10 Define constraint residuals:
$r_{j}(\boldsymbol{\xi}^{*}_{j})=\sum\max(\textbf{0},\textbf{g}(\boldsymbol{\xi}^{*}_{j}))$.

11 $ConstraintEliteSet\leftarrow$ Select top $n$ samples of
$\textbf{p}_{j},\boldsymbol{\xi}^{*}_{j}$ with lowest constraint residuals.

12 $cost\leftarrow$
$c_{u}(\boldsymbol{\xi}_{j}^{*})+r_{j}(\boldsymbol{\xi}^{*}_{j})$, over
$ConstraintEliteSet$

13 append ${cost}$ to $CostList$

14

15 $EliteSet\leftarrow$ Select top $q$ samples of
($\textbf{p}_{j},\boldsymbol{\xi}^{*}_{j}$) with lowest cost from $CostList$.

16

17
$({{}^{l+1}}\boldsymbol{\mu}_{p},{{}^{l+1}}\boldsymbol{\Sigma}_{p})\leftarrow$
Update distribution based on $EliteSet$

18 end for

return _Parameter $\textbf{p}_{j}$ and $\boldsymbol{\xi}^{*}_{j}$
corresponding to lowest
$c_{u}(\boldsymbol{\xi}^{*}_{j})+r_{j}(\boldsymbol{\xi}^{*}_{j})$ in the
$EliteSet$_

Algorithm 1 Bi-Level Optimization

###  III-C Computational Tractability of Alg.1

The main computational bottleneck of Alg. 1 stems from the requirement of
solving a large number ($\overline{n}\approx 1000$) of non-convex
optimizations on line 6-8. However, each of these optimizations is decoupled
from the other; thus, we can solve them in parallel (a.k.a., the batch
setting) to ensure computational tractability. To this end, we first consider
the QP presented on line 7. Solving it for the $j^{th}$ sample of
$\textbf{p}_{j}$ reduces to following linear equations:

$\displaystyle\begin{bmatrix}\textbf{Q}&\textbf{A}^{T}\\\
\textbf{A}&\textbf{0}\end{bmatrix}\begin{bmatrix}\boldsymbol{\xi}_{j}\\\
\boldsymbol{\mu}_{j}\end{bmatrix}=\begin{bmatrix}\textbf{q}(\textbf{p}_{j})\\\
\textbf{b}(\textbf{p}_{j})\end{bmatrix},$ (7)

where, $\boldsymbol{\mu}_{j}$ is the dual variable associated with the
equality constraints. The left-hand side of (7) is constant and independent of
the $\textbf{p}_{j}$. Thus, the solution for the entire batch can be
constructed in one shot in the following manner:

Figure 3: a) Our BC framework uses a combination of conventional feed-forward
and differentiable optimization layers. b) To learn a good initialization
distribution, a CVAE is trained to approximate the optimal distribution of p
that results in prediction $\boldsymbol{\xi}^{*}$ as close as possible to
expert demonstrations $\boldsymbol{\xi}_{e}$.

$\displaystyle\begin{bmatrix}\begin{array}[]{<EMAIL_ADDRESS>\boldsymbol{\mu}_{1}&...&\boldsymbol{\mu}_{\overline{n}}\end{array}\end{bmatrix}=\overbrace{(\begin{bmatrix}\textbf{Q}&\textbf{A}^{T}\\\
\textbf{A}&\textbf{0}\end{bmatrix}^{-1})}^{constant}\begin{bmatrix}\begin{array}[]{@{}c|c|cc@{}}\textbf{q}(\textbf{p}_{1})&...&\textbf{q}(\textbf{p}_{\overline{n}})\\\
\textbf{b}(\textbf{p}_{1})&...&\textbf{b}(\textbf{p}_{\overline{n}})\end{array}\end{bmatrix},$
(8)

The inverse on the right-hand side of (8) needs to be computed only once,
irrespective of the batch size. Thus the batch solution of QP reduces to just
a large matrix-vector product that can be trivially accelerated over GPUs.

For handling the projection operation on line 8 of Alg. 1, we build on our
recent work [10]. This work showed that the core numerical algebra associated
with projecting sampled trajectories on the collision avoidance constraints
and motion bounds has the same batch QP structure as (7) and (8). We extend
[10] to include curvature, centripetal acceleration bounds and lane boundary
constraints while retaining its efficient batch projection update rule. We
present a detailed derivation in Appendix D.

###  III-D Learning Good Initialization Distribution

In this section, we derive a Behaviour Cloning (BC) framework to learn a
neural-network policy that maps observations o to optimal parameters p.
Typically in BC, we assume that we have access to a data set
$(\textbf{o},\boldsymbol{\xi}_{e})$ that demonstrates the expert (optimal)
trajectory $\boldsymbol{\xi}_{e}$ for each observation vector o. We cannot
directly have access to a demonstration of the optimal behavioural parameter p
employed by the expert. Rather, we have their indirect observation through
$\boldsymbol{\xi}_{e}$. Thus, our problem setting is more complicated than the
typical BC setup.

We address the mentioned challenges by using a network architecture that is a
combination of conventional feed-forward, and differentiable optimization
layers [11]. An overview of the main concept is presented in Fig.3 (a). The
learnable weights are only present in the feed-forward layer. It takes in
takes in observations o to output the behavioural parameter p, which is then
fed to an optimizer resulting in an optimal trajectory $\boldsymbol{\xi}^{*}$.
The BC loss is computed over $\boldsymbol{\xi}^{*}$. The backpropagation
required for updating the weights of the feed-forward layer needs to trace the
gradient of the loss function through the optimization layer.

#### Need for CVAE

We want our learned policy to induce a distribution over p so that for each
observation o, we can then draw samples from it to initialize our bi-level
optimizer presented in Alg. 1 (line 4). With this motivation, we use a deep
generative model called CVAE [12], illustrated in Fig.3 as our learning
pipeline. It consists of an encoder and decoder architecture constructed from
a multi-layer perceptron (MLP) with weights $\boldsymbol{\phi}$ and
$\boldsymbol{\theta}$ respectively. Additionally, the decoder network has an
optimization layer that takes the output (p) of its MLP to produce an estimate
of an optimal trajectory $\boldsymbol{\xi}^{*}$.

Let z be a latent variable such that the $p_{\theta}(\textbf{z})$ represents a
isotropic normal distribution($\mathcal{N}(0,\textbf{I})$). The decoder
network maps this distribution to
$p_{\theta}(\boldsymbol{\xi}^{*}|\textbf{z},\textbf{o})$. The encoder network
on the other hand maps $(\textbf{o},\boldsymbol{\xi}_{e})$ to a distribution
$q_{\phi}(\textbf{z}|\textbf{o},\boldsymbol{\xi}^{*})$ over z. In the offline
phase, both the networks are trained end-to-end with loss function (9). The
first term is the reconstruction loss responsible for bringing the output of
the decoder network as close as possible to the expert trajectory. The second
term in (9) acts as a regularizer that aims to make the learned latent
distribution $q_{\phi}(\textbf{z}|\textbf{o},\boldsymbol{\xi}_{e})$ as close
as possible to the prior normal distribution. The $\boldsymbol{\beta}$
hyperparameter acts as a trade-off between the two cost terms. The detailed
architecture of our CVAE, alongside the training hyperparameters, is presented
in Appendix. A.

${\textit{L}}_{\textit{CVAE}}=\underset{\boldsymbol{\theta},\boldsymbol{\phi}}{min}\sum\|\boldsymbol{\xi}^{*}(\boldsymbol{\theta},\boldsymbol{\phi})-\boldsymbol{\xi}_{e}\|_{2}^{2}\newline
+\beta\,{D}_{\mathbf{KL}}[q_{\phi}(\textbf{z}\,|\,\textbf{o},\,\boldsymbol{\xi}_{e})\,|\,p_{\theta}(\textbf{z})]$
(9)

In the inferencing (online) phase, we draw samples of z from an isotropic
normal distribution and then pass them through the decoder MLP to get samples
of optimal behavioural parameter p. Finally, the parameter samples are passed
through the optimizer to generate distribution for the optimal trajectory
$\boldsymbol{\xi}^{*}$.

Figure 4: Fig.(a) shows the trajectory distribution resulting from sampling
behavioural inputs p from a Gaussian distribution. Fig.(b) shows the
corresponding distribution when sampling p from a learned CVAE. As can be
seen, CVAE results in a more structured and smoother trajectory distribution
concentrated around the expert demonstration, shown in black. The trajectory
samples also conform to lane boundaries shown in dotted black lines. The blue
rectangle represents the neighbouring vehicles moving along straight-line
(blue) trajectories. The green rectangle represents the ego-vehicle.

####  III-D1 Choice of Differentiable Optimizer

Ideally, we should embed the entire lower-level trajectory optimizer (5b)-(5c)
into the CVAE decoder architecture. However, backpropagating through such non-
convex optimization is fraught with technical difficulties. In fact, the
greatest success of learning with optimization layers has come while embedding
convex optimizers into neural networks [11]. Thus, we adopt a simplification
as shown in Fig.3(b). We construct our optimiztion layer with convex cost (5b)
and affine equality constraints in (5c), both of which depend explicitly on p.
We ignore the inequality constraints, where the parameters do not appear. The
intuition behind our choice is that the expert trajectory will be collision-
free and kinematically feasible and thus would automatically satisfy the
inequality constraints. Thus, we need to figure out the right set of p that
can mimic the expert behaviour as closely as possible.

####  III-D2 Example of a CVAE Output

Fig.4 contrasts initialization of Alg. 1 from a naive Gaussian distribution
and our learned CVAE. For Fig.4(a), we sampled p from a Gaussian distribution
and solved the QP presented in line 7 of Algorithm 1, resulting in a
distribution of trajectories. We repeated the same process for the samples
drawn from CVAE in Fig.4(b). It can be clearly seen that the distribution
resulting from p drawn from CVAE is smoother, conforms to lane boundaries, and
is concentrated around the expert demonstration trajectory.

##  IV Connections to Existing Works

#### Trajectory Sampling Approaches

Existing works like [5], [7] can be viewed as a special case of our Alg. 1
obtained by performing only one iteration of the bi-level optimizer. These
cited works sample the parameter p (lateral offsets, forward velocity, etc.),
albeit not from a Gaussian distribution but a pre-discretized grid. This is
followed by the execution of lines 6-8 and ranking of the upper-level cost
(line 13) associated with the generated trajectories. However, [5], [7] do not
have any mechanism to adapt the sampling distribution (or grid) to reduce the
upper-level cost. Authors in [8] address this drawback to some extent as they
adapt the sampling strategy based on optimal trajectories obtained in the past
planning cycles. Such an adaptation strategy would be akin to performing one
iteration of Alg. 1 and then warm-starting the sampling distribution of p at
the next planning cycle with the updated mean and variance obtained from line
14.

The lower-level planners of [5], [7], [8] ignore inequality constraints and
essentially solve the QP presented on line 7 of Alg. 1. The constraint
residuals (e.g. obstacle clearance) are augmented into the cost function,
similar to line 11. Our current work includes an additional projection
operator at line 8 of Algorithm 1 to aid in constraint satisfaction.

#### RL Based Approaches

Works like [1], [2], [3], [4] can be viewed as training a function
approximator to learn the solution space of the bi-level optimizer presented
in 1. In Section III-D, we have made a similar attempt using a supervised
setting. The RL approaches of [1], [2], [3], [4] would achieve this in a self-
supervised setting based on just feedback of reward (upper-level cost) from
the environment.

####  Bi-level Optimization

The bi-level approaches are extensively used in motion planning. For example,
see [13], [14]. Our work is closely related to the latter. In [14], an offline
bi-level optimization is used to generate optimal higher-level behaviours
(parameter p) for drones and subsequently, a neural network is trained to
learn this solution space. At run-time, the neural network’s output is the
true solution. In sharp contrast, we take the output from the CVAE trained in
Section III-D as just a guess for the optimal parameter and adapt it in real-
time in Alg.1.

#### Comparison with Gradient Descent

Bi-level optimizations are commonly solved through Gradient Descent [13]. It
requires computing the Jacobian of the optimal solution $\boldsymbol{\xi}^{*}$
with respect to parameter p through implicit differentiation [11]. The main
drawback of this approach is that implicit differentiation has technical
difficulties in case there are multiple local minima and/or when the lower-
level problem is infeasible. In contrast, our Alg. 1 does not require the
lower-level optimization to be feasible and allows for its early termination.
In either case, the constraint residuals can measure the quality of the
optimal trajectory, which is why we augment it into the upper-level cost on
line 11.

##  V Experiments

###  V-A Implementation Details

We implemented Alg. 1 including the lower-level optimizer in Python using JAX
[15] library as our GPU-accelerated linear algebra back-end. The matrix W in
(1) is constructed from a $10^{th}$ order polynomial. Our simulation pipeline
was built on top of the Highway Environment (highway-env) simulator [16]. The
neighbouring vehicles used IDM [17] for longitudinal and MOBIL [18] for
lateral control.

####  V-A1 Hyper-parameter Selection

The sampling size $\overline{n}$ in Alg. 1 was 1000. The $ConstraintEliteSet$
(line 10, Alg. 1) and $EliteSet$ (line 13, Alg. 1) had 150 and 50 samples
respectively. For our bi-level optimizer, the behavioural input p was modelled
as four set-points for lateral offsets and desired longitudinal velocities.
That is,
$\textbf{p}=\begin{bmatrix}y_{d,1},\dots,y_{d,4},v_{d,1},\dots,v_{d,4}\end{bmatrix}$.
We divided the planning horizon into four segments and associated one pair of
lateral offset and desired velocity to each of these. We recall that the Alg.
1 computes the optimal p along with the associated trajectory. $\gamma$ value
in (6c) was 0.9.

Figure 5: Two-lane and four-lane driving scenarios with varying traffic
density for benchmarking our approach with MPC and RL baselines.

####  V-A2 CVAE Training

The details of the encoder and decoder network architecture of our CVAE are
presented in the accompanying video and Appendix A. During training, the input
to the CVAE is the expert trajectory and a 55-dimensional observation vector
(o) containing the state of the ego-vehicle, the ten closest obstacles and the
road boundary. For the ego-vehicle, the state consists of a heading, lateral
and longitudinal speeds. The state consists of longitudinal and lateral
positions and the corresponding velocities for the ten closest obstacles. We
express all the position-level information with respect to the centre of the
ego vehicle. During inferencing, the decoder network only needs o and samples
of z drawn from an isotropic Gaussian.

We used the cross-entropy method, run offline with a batch size of 1000, to
collect the demonstration of optimal trajectories for training our CVAE. We
note that our demonstrations could possibly be sub-optimal. However, even with
such a simple data set, our CVAE is able to learn useful initialization for
our bi-level optimizer presented in Alg.1.

####  V-A3 MPC Baselines

We also used Alg.1 in a receding horizon manner to create an MPC variant of
our bi-level optimizer. We will henceforth refer to it as MPC-Bi-Level. It
takes the same observation vector o as the CVAE and outputs polynomial
coefficients of the optimal trajectories. These are converted to steering and
acceleration input vectors, and the ego-vehicle executes the first five
elements of these in the open loop before initiating the next re-planning. We
compare our MPC-Bi-Level with the following baselines.

  * •

MPC-Vanilla: This baseline runs without a behavioural layer. We only have the
lower-level optimization (5b)-(5c). The parameter p is a scalar representing
the set-point for the desired longitudinal velocity.

  * •

MPC-Grid: This baseline uses the same set of behavioural parameters p as our
MPC-Bi-Level but samples them from a pre-specified fixed grid.

  * •

MPC-Random: This baseline is similar to MPC-Grid but samples p from a Gaussian
distribution.

  * •

Batch-MPC of [19]: This baseline is similar to MPC-Grid but uses a different
set of behavioural inputs, namely goal positions for the longitudinal and
lateral components of the trajectory. That is,
$\textbf{p}=\begin{bmatrix}x_{f},y_{f}\end{bmatrix}$.

Figure 6: Empirical validation of Alg.1. The top of Fig.(a),(b) shows the
trajectory distribution resulting from sampling behavioural inputs p from a
Gaussian distribution and solving the QP at line 7 of Alg.1. The bottom plots
in both figures show the modified distribution after projection onto the
constrained set. Fig.(a),(b) correspond to the distribution at the first and
the fifth iteration, respectively. As the iterations progress, the variance of
the trajectory distribution shrinks, and the upper-level cost (Fig.(c), top)
saturates, indicating convergence of our bi-level optimizer. Fig.(c) (bottom)
shows the constraint residual of the lower-level optimization at each
iteration.

####  V-A4 RL Baselines

We also compare our approach against Deep Q-Network (DQN) and Proximal Policy
Optimization (PPO), developed using the framework outlined in [20] and [21]
respectively. The input observation is the same as our MPC-Bi-Level. The
action space is discrete with 5 different behaviours, namely faster, slower,
left-lane change, idle, right-lane change. These behaviours are then mapped to
a set-point for lateral offset or longitudinal velocity and tracked through a
PID controller using appropriate steering and acceleration commands. Both DQN
and PPO have been trained on the highway-env simulator using Stable-Baselines
3 [22]. See Appendix C for further details.

####  V-A5 Environments, Tasks, and Metrics

The driving scenarios are presented in Fig.5. For each scenario, we had two
different traffic densities. We use the internal parameter of HighEnv named
”density” to control how closely each vehicle is placed at the start of the
simulation. The RL baselines did not perform well in very dense environments
and thus were tested in sparser environments than the MPC-based approaches. In
each scenario, we evaluated 50 episodes with different randomly initialized
traffic. We fixed the random seed of the simulator to ensure that all RL and
MPC baselines are tested across the same set of traffic configurations.

The task in the experiment was for the ego-vehicle to drive as fast as
possible without colliding with the obstacles and going outside the lane
boundary. Thus, the upper-level cost of our bi-level optimizer has the form
${(\sqrt{\dot{x}^{*^{2}(t)}+\dot{y}^{*^{2}(t)}}}-v_{max})^{2}$, where
$({x}^{*(t)},{y}^{*(t)})$ are the optimal velocity profiles obtained from the
lower-level optimization. Note that these are obtained from
$\boldsymbol{\xi}^{*}$ through relationship (1). The safety is handled by the
constraints of the lower-level optimization. Our evaluation metric has two
components: (i) collision rate and (ii) average velocity achieved within an
episode. Since the ego-vehicle can achieve arbitrary high velocity while
driving rashly, we only consider velocities from collision-free episodes.

###  V-B Empirical Validation of Convergence

Fig.6 shows the performance of our bi-level optimizer presented in Alg.1 on a
typical environment with static obstacles. The top plot of Fig.(a), (b) shows
the trajectory distributions at the first and the fifth iteration,
respectively, resulting from sampling the behavioural inputs p from a Gaussian
distribution and solving the QP presented in line 7 of Alg.1. The bottom plots
in these figures show how the distribution changes when we project them onto
the feasible set of collision avoidance and kinematic constraints. The
following key observations can be made from Fig.6:

  * •

The projection operation results in trajectories residing in different
homotopies, which in turn proves crucial for proper exploration of the state
space.

  * •

The variance of the trajectory distribution shrinks, and the upper-level cost
(Fig.6(c)(top)) saturates. This is a typical convergent behaviour observed in
sampling-based optimizers such as Alg.1. Please note that shrinking of
trajectory variance in Fig.6(a), (b) (top) also signifies that the sampling
distribution for p has also converged to an optimal one.

  * •

Finally, Fig.6(c)(bottom) validates the role of our projection operator.

###  V-C Importance of Bi-Level Adaptation

Figure 7: Comparison of MPC Bi-Level (Ours) with other MPC-based baselines in
two (a, c) and four-lane (b, d) driving scenarios.

In a two-lane driving scenario, there is only a limited set of manoeuvres that
the ego-vehicle can do. Thus, on a low traffic density, all the baselines
except MPC-Random achieve perfect collision rate (Fig.7 (a)). This shows that
it is not critical to have a dedicated behavioural layer on simple driving
scenarios. Our observation is not surprising as existing results like [23],
[24] have shown promising results without explicitly incorporating behavioural
inputs, but in very sparse environments. Fig.7(a) also shows that in a sparse
two-lane environment, a simple grid-search used by MPC-Grid and Batch-MPC [19]
is enough to come up with the right set of behavioural inputs. However, as
traffic density increases in the two-lane setting, the safety improvement
provided by our MPC-Bi-Level becomes distinctly apparent. The trend is
particularly stark in highly dense four-lane driving scenarios, where our
approach provides a 4-10x reduction in collision rate over other baselines.
Fig.7(b), (d) shows that the average speed achieved by our MPC-Bi-Level is
generally either better or competitive with all the baselines.

###  V-D Safety Improvements over RL

We trained both DQN and PPO for over 5 million steps. However, we could make
them work reasonably in only sparse two-lane traffic. Nevertheless, the
collision rate and velocity (Fig.8(a, c) ) achieved by DQN and PPO were still
drastically worse than our MPC-Bi-Level. The performance gap increased further
in a dense four-lane setting, as shown in Fig.8(b, d).

Figure 8: Comparison of MPC Bi-Level (Ours) with other RL-based baselines in
two (a, c) and four-lane (b, d) driving scenarios.

###  V-E Effect of CVAE Initialization

Fig.9(a) shows that for a relatively small batch size of 250, the learned CVAE
initializer achieves a 4x reduction in collision rate over the naive Gaussian
distribution. However, the performance gap between both initializations
reduces a bit as we increase the batch size (Fig.9(b)). Thus, both
Fig.9(a)-(b) validate that a learned CVAE initializer can be particularly
helpful when Alg. 1 is run with a limited computation budget or on resource-
constrained hardware, wherein we have to contend with a smaller batch size for
Alg.1.

Figure 9: Computational advantage achieved by initializing Alg. 1 through
samples drawn from learnt CVAE over baseline Gaussian distribution. CVAE
allows us to achieve a better collision rate with a smaller batch size.

###  V-F Computation Time

Fig.10 shows the computation time requirement for our Alg.1 on a laptop with
RTX 3080 GPU. In sparse traffic scenarios, two iterations of Alg.1 proved
enough to achieve a low collision rate when initialized with the learned CVAE.
For a batch size of 250, this corresponds to a feedback rate of around 100 Hz.
All the benchmarking presented in Section V-C, V-D were obtained with the same
batch size but used 5 iterations of Alg.1, totalling to $0.03s$. Fig.10(b)
demonstrates a moderate increase in the computation time with respect to batch
size; even for a batch size of 1000, the computation time was less than
$0.06s$. Finally, both Fig.10(a), (b) shows that the additional overhead of
inferencing the learned CVAE is very minimal.

Figure 10: Samples drawn from learnt CVAE distribution leads to better
collision-rate without the expense of computational time.

##  VI Conclusion and Future Work

We proposed a novel bi-level optimization that can simultaneously search for
the optimal higher-level behavioural decisions along with the lower-level
trajectories necessary for executing them. Our custom optimizer combines
features from gradient-free, sampling-based optimization with QP and runs in
real time due to an efficient GPU parallelization of the lower-level
optimization. We also proposed a CVAE architecture constructed from a feed-
forward neural network and differentiable optimization layers to learn good
initialization for our bi-level optimizer.

We conducted extensive experiments to showcase the importance of having a
dedicated behavioural layer. Our approach also outperformed competing MPC and
RL baselines. Finally, the learned CVAE initialization improved the
computational tractability of our bi-level optimizer by reducing the batch
size and number of iterations required to achieve a given collision rate.

Our bi-level optimizer sets the groundwork for an RL framework where the
policy is a combination of a neural network that provides higher-level
decisions and a local MPC. We conjecture that an Augmented Random Search
technique [25] for training both the network and MPC in an end-to-end fashion
will have a very similar structure as Alg.1.

## References

  * [1] C.-J. Hoel, K. Driggs-Campbell, K. Wolff, L. Laine, and M. J. Kochenderfer, “Combining planning and deep reinforcement learning in tactical decision making for autonomous driving,” _IEEE transactions on intelligent vehicles_ , vol. 5, no. 2, pp. 294–305, 2019.
  * [2] S. Han and F. Miao, “Behavior planning for connected autonomous vehicles using feedback deep reinforcement learning,” _arXiv preprint arXiv:2003.04371_ , 2020.
  * [3] M. Huegle, G. Kalweit, B. Mirchevska, M. Werling, and J. Boedecker, “Dynamic input for deep reinforcement learning in autonomous driving,” in _2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2019, pp. 7566–7573.
  * [4] J. Li, L. Sun, J. Chen, M. Tomizuka, and W. Zhan, “A safe hierarchical planning framework for complex driving scenarios based on reinforcement learning,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 2660–2666.
  * [5] J. Wei, J. M. Snider, T. Gu, J. M. Dolan, and B. Litkouhi, “A behavioral planning framework for autonomous driving,” in _2014 IEEE Intelligent Vehicles Symposium Proceedings_. IEEE, 2014, pp. 458–464.
  * [6] W. Lim, S. Lee, M. Sunwoo, and K. Jo, “Hybrid trajectory planning for autonomous driving in on-road dynamic scenarios,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 1, pp. 341–355, 2019.
  * [7] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in _2010 IEEE International Conference on Robotics and Automation_. IEEE, 2010, pp. 987–993.
  * [8] S. Sun, Z. Liu, H. Yin, and M. H. Ang, “Fiss: A trajectory planning framework using fast iterative search and sampling strategy for autonomous driving,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9985–9992, 2022\.
  * [9] M. Bhardwaj, B. Sundaralingam, A. Mousavian, N. D. Ratliff, D. Fox, F. Ramos, and B. Boots, “Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation,” in _Conference on Robot Learning_. PMLR, 2022, pp. 750–759.
  * [10] H. Masnavi, J. Shrestha, M. Mishra, P. Sujit, K. Kruusamäe, and A. K. Singh, “Visibility-aware navigation with batch projection augmented cross-entropy method over a learned occlusion cost,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9366–9373, 2022.
  * [11] B. Amos and J. Z. Kolter, “Optnet: Differentiable optimization as a layer in neural networks,” in _International Conference on Machine Learning_. PMLR, 2017, pp. 136–145.
  * [12] D. P. Kingma and M. Welling, “Auto-encoding variational $\\{$Bayes$\\}$,” in _Int. Conf. on Learning Representations_.
  * [13] W. Sun, G. Tang, and K. Hauser, “Fast uav trajectory optimization using bilevel optimization with analytical gradients,” in _2020 American Control Conference (ACC)_. IEEE, 2020, pp. 82–87.
  * [14] Y. Song and D. Scaramuzza, “Policy search for model predictive control with application to agile drone flight,” _IEEE Transactions on Robotics_ , 2022\.
  * [15] J. Bradbury, R. Frostig, P. Hawkins, M. J. Johnson, C. Leary, D. Maclaurin, G. Necula, A. Paszke, J. VanderPlas, S. Wanderman-Milne, and Q. Zhang, “JAX: composable transformations of Python+NumPy programs,” 2018. [Online]. Available: http://github.com/google/jax
  * [16] E. Leurent, “An Environment for Autonomous Driving Decision-Making,” 5 2018\. [Online]. Available: https://github.com/eleurent/highway-env
  * [17] M. Treiber, A. Hennecke, and D. Helbing, “Congested traffic states in empirical observations and microscopic simulations,” _Physical review E_ , vol. 62, no. 2, p. 1805, 2000.
  * [18] A. Kesting, M. Treiber, and D. Helbing, “General lane-changing model mobil for car-following models,” _Transportation Research Record_ , vol. 1999, no. 1, pp. 86–94, 2007.
  * [19] V. K. Adajania, A. Sharma, A. Gupta, H. Masnavi, K. M. Krishna, and A. K. Singh, “Multi-modal model predictive control through batch non-holonomic trajectory optimization: Application to highway driving,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4220–4227, 2022.
  * [20] C.-J. Hoel, K. Wolff, and L. Laine, “Automated speed and lane change decision making using deep reinforcement learning,” in _2018 21st International Conference on Intelligent Transportation Systems (ITSC)_. IEEE, 2018, pp. 2148–2155.
  * [21] T. Liu, H. Wang, B. Lu, J. Li, and D. Cao, “Decision-making for autonomous vehicles on highway: Deep reinforcement learning with continuous action horizon,” _arXiv preprint arXiv:2008.11852_ , 2020.
  * [22] A. Raffin, A. Hill, A. Gleave, A. Kanervisto, M. Ernestus, and N. Dormann, “Stable-baselines3: Reliable reinforcement learning implementations,” _Journal of Machine Learning Research_ , vol. 22, no. 268, pp. 1–8, 2021\. [Online]. Available: http://jmlr.org/papers/v22/20-1364.html
  * [23] B. Gutjahr, L. Gröll, and M. Werling, “Lateral vehicle trajectory optimization using constrained linear time-varying mpc,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 18, no. 6, pp. 1586–1595, 2016.
  * [24] P. Lin and M. Tsukada, “Model predictive path-planning controller with potential function for emergency collision avoidance on highway driving,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4662–4669, 2022\.
  * [25] H. Mania, A. Guy, and B. Recht, “Simple random search of static linear policies is competitive for reinforcement learning,” _Advances in Neural Information Processing Systems_ , vol. 31, 2018.
  * [26] I. Loshchilov and F. Hutter, “Decoupled weight decay regularization,” in _International Conference on Learning Representations_ , 2019. [Online]. Available: https://openreview.net/forum?id=Bkg6RiCqY7
  * [27] S. R. Bowman, L. Vilnis, O. Vinyals, A. Dai, R. Jozefowicz, and S. Bengio, “Generating sentences from a continuous space,” in _Proceedings of the 20th SIGNLL Conference on Computational Natural Language Learning_. Berlin, Germany: Association for Computational Linguistics, Aug. 2016, pp. 10–21. [Online]. Available: https://aclanthology.org/K16-1002
  * [28] G. Taylor, R. Burmeister, Z. Xu, B. Singh, A. Patel, and T. Goldstein, “Training neural networks without gradients: A scalable admm approach,” in _International conference on machine learning_. PMLR, 2016, pp. 2722–2731.
  * [29] E. Ghadimi, A. Teixeira, I. Shames, and M. Johansson, “Optimal parameter selection for the alternating direction method of multipliers (admm): quadratic problems,” _IEEE Transactions on Automatic Control_ , vol. 60, no. 3, pp. 644–658, 2014.

##  Appendix A A. CVAE Architecture & Training Hyperparameters

Encoder Network
---
Block |  Layers |  Output Size |  Activation
MLP 1 - 4 | Linear, Batchnorm | 1024 | ReLU
MLP 5 | Linear, Batchnorm | 256 | ReLU
Mean | Linear | 2 | None
Variance | Linear | 2 | Softplus

Decoder Network
---
Block |  Layers |  Output Size |  Activation
MLP 1 - 4 | Linear, Batchnorm | 1024 | ReLU
MLP 5 | Linear, Batchnorm | 256 | ReLU
p | Linear | 8 | None
$\boldsymbol{\xi^{*}}$ | Optimization Layer | 22 | None

TABLE I: CVAE architecture composed of Encoder and Decoder Networks
illustrated in Fig.3

The optimizer used for training the CVAE was AdamW [26] with a learning rate
of 1e-4 and weight decay of 6e-5 for a total of 80 epochs. Moreover, the
learning rate was decayed by $\gamma=0.1$ every 10 epochs. We tackle the KL-
vanishing issue by applying a monotonic annealing schedule of
$\boldsymbol{\beta}$ coefficient [27], starting with 0 and gradually annealing
the $\boldsymbol{\beta}$ at each step.

##  Appendix B B. Additional Experiments on Two-Way Environment

Figure 11: Two-way highway driving scenario for benchmarking our approach with
MPC and RL-based baselines.

DQN and PPO fail to work reasonably in a relatively simple Two-Way highway
driving scenario Fig.11. MPC-based approaches, including our MPC Bi-Level,
achieved zero collision rate and higher velocity than RL baselines, as shown
in Fig.12 (a, b).

Figure 12: Comparison of MPC Bi-Level with other RL-based baselines in two-way
(a, b) driving scenario.

##  Appendix C RL Training Hyperparameters

The policy estimators employed in RL baselines are MLPs with two hidden layers
of 256 neurons each. The action space is discrete with the following different
behaviours:

  * •

faster: Increase velocity by $5$ m/s

  * •

slower: Decrease velocity by $5$ m/s

  * •

left-lane change: change lateral position by $4$ m to the left of the current
position.

  * •

idle: Keep moving with the current velocity and lateral offset set-points.

  * •

right-lane change: change lateral position by $4$ m to the right of the
current position.

The complete set of RL training hyper-parameters are summed up in Table II.

TABLE II: RL baselines training parameters Agent | Parameter | Value
---|---|---
| Number of training steps | 5M
| Policy Scheduling Time | 1s
| Input neurons | 55
| Hidden layers | 2
| Hidden layers neurons | 256
| Output neurons | 5
| Discount factor | 0.8
| Learning rate | 5e-4
DQN | Replay Memory size | 15k
| Initial exploration constant | 1
| Final exploration constant | 0.1
| Target Network update frequency | 50
| Batch size | 32
PPO | number of steps | 10
| Batch size | 64
| Generalized Advantage Estimation(GAE) $\lambda$ | 0.95
| clipping coefficient | 0.2
| value-function coefficient | 0.5

##  Appendix D GPU Accelerated Batch Projection

In this section, we modify the projection operator of [10] to make it suitable
for car-like vehicles while retaining the batch QP structure presented in (8).
We begin by summarizing the set of inequality constraints used in the lower-
level optimization ($g_{j}(x(t))\leq 0$) in Table III.

TABLE III: List of Inequality Constraints Used in the lower level optimization Constraint Type | Expression | Parameters
---|---|---
Collision Avoidance | $-\frac{(x(t)-x_{o,i})^{2}}{a^{2}}-\frac{(y(t)-y_{o,i})^{2}}{b^{2}}+1\leq 0$ |  $\frac{a}{2},\frac{b}{2}$: axis of the circumscribing ellipse of vehicle footprint. $x_{o,i}(t),y_{o,i}(t)$: trajectory of neighboring vehicles
Velocity bounds | $\sqrt{\dot{x}(t)^{2}+\dot{y}(t)^{2}}\leq v_{max}$ |  $v_{max}$: maximum velocity of the ego-vehicle
Acceleration bounds | $\sqrt{\ddot{x}(t)^{2}+\ddot{y}(t)^{2}}\leq a_{max}$ |  $a_{max}$: maximum acceleration of the ego-vehicle
Curvature bounds | $-\kappa_{max}\leq|\frac{\ddot{y}(t)\dot{x}(t)-\ddot{x}(t)\dot{y}(t)}{(\dot{x}(t)^{2}+\dot{y}(t)^{2})^{1.5}}|\leq\kappa_{max}$ |  $-\kappa_{max}$: maximum curvature bound for the ego-vehicle trajectory
Centripetal Acceleration bounds | $-c_{max}\leq\dot{x}(t)^{2}\kappa(x(t))\leq c_{max}$ |  $c_{max}$: maximum centripetal acceleration bound for the ego-vehicle.
Lane boundary | $y_{lb}(x(t))\leq y(t)\leq y_{ub}(x(t))$ |  $y_{lb},y_{ub}$: Lane limits as a function of the ego-vehicle’s position.

###  D-A Reformulating Quadratic Inequalities

The collision avoidance constraints can be re-written in the following form:

$\displaystyle\textbf{f}_{o,i}=\left\\{\begin{array}[]{lcr}x(t)-x_{o,i}(t)-d_{o,i}(t)\cos\alpha_{o,i}(t)\\\
y(t)-y_{o,i}(t)-d_{o,i}(t)\sin\alpha_{o,i}(t)\\\
\end{array}\right\\}d_{o,i}(t)\geq 1$ (12)

where $\alpha_{o,i}(t)$ represents the angle that the line-of-sight vector
between the ego-vehicle and its $i^{th}$ neighbor makes with the $X$ axis.
Similarly, the variable $d_{o,i}(t)$ represents the ratio of the length of
this vector with the minimum distance separation required for collision
avoidance. Following a similar approach, we can rephrase the velocity and
acceleration bounds from Table III as:

$\displaystyle\textbf{f}_{v}=\left\\{\begin{array}[]{lcr}\dot{x}(t)-d_{v}(t)\cos\alpha_{v}(t)\\\
\dot{y}(t)-d_{v}(t)\sin\alpha_{v}(t)\\\ \end{array}\right\\},v_{min}\leq
d_{v}(t)\leq v_{max}$ (15)

$\displaystyle\textbf{f}_{a}=\left\\{\begin{array}[]{lcr}\ddot{x}(t)-d_{a}(t)\cos\alpha_{a}(t)\\\
\ddot{y}(t)-d_{a}(t)\sin\alpha_{a}(t)\\\ \end{array}\right\\},0\leq
d_{a}(t)\leq a_{max}$ (18)

The variables $\alpha_{o,i}(t)$, $\alpha_{o,i}(t)$, $\alpha_{a,i}(t)$,
$d_{o,i}(t)$, $d_{v,i}(t)$, and $d_{a,i}(t)$ are additional variables that
will be obtained by our batch projection optimizer along with
$\boldsymbol{\xi}_{j}$.

Using (15)-18, curvature and centripetal acceleration can also be reduced to
the (19a) and (19b) respectively.

$\displaystyle\frac{d_{a}(t)|\sin(\alpha_{a}(t)-\alpha_{v}(t)|}{d_{v}(t)^{2}}\leq\kappa_{max}$
(19a) $\displaystyle{d}_{v}(t)^{2}\cos\alpha_{v}(t)^{2}|\kappa(x(t))|\leq
c_{max}$ (19b)

The variable $\kappa(x(t))$ represents the curvature of the road. We
approximate it as the curvature of the reference reference center-line aligned
with the road geometry. Note that $\kappa(x(t))$ depends on the ego-vehicles
position $x(t)$ with respect to the center-line.

####  D-A1 Reformulated Problem

Using the developments in the previous section and the trajectory
parametrization presented in (1), we can now replace the projection operator
on line 8 of Alg. 1 with the following. Note that (20g) is the matrix
representation of the lane boundary constraints presented in Table III.

$\displaystyle\boldsymbol{\xi}_{j}^{*}=\arg\min_{{\boldsymbol{\xi}}_{j}}\frac{1}{2}\|\boldsymbol{\xi}_{j}-\overline{\boldsymbol{\xi}}_{j}\|_{2}^{2}$
(20a) $\displaystyle\textbf{A}_{eq}{\boldsymbol{\xi}}_{j}=\textbf{b}_{eq}$
(20b)
$\displaystyle\textbf{F}\boldsymbol{\xi}_{j}=\textbf{h}_{j}(\boldsymbol{\alpha}_{j},\textbf{d}_{j})$
(20c) $\displaystyle\textbf{d}_{o,j}\geq 1,\hskip
2.84544ptv_{min}\leq\textbf{d}_{v,j}\leq v_{max},\hskip
2.84544pt0\leq\textbf{d}_{a,j}\leq a_{max}$ (20d)
$\displaystyle\textbf{d}_{a,j}|\sin(\boldsymbol{\alpha}_{a,j}-\boldsymbol{\alpha}_{v,j})|\leq\textbf{d}_{v,j}^{2}\kappa_{max}$
(20e)
$\displaystyle(\textbf{d}_{v,j}\cos\boldsymbol{\alpha}_{v,j})^{2}\kappa(\boldsymbol{\xi}_{j})\leq
c_{max}.$ (20f)
$\displaystyle\textbf{G}\boldsymbol{\xi}_{j}\leq\textbf{e}(\boldsymbol{\xi}_{j})$
(20g)

$\displaystyle\textbf{F}=\begin{bmatrix}\begin{bmatrix}\textbf{F}_{o}\\\
\dot{\textbf{W}}\\\ \ddot{\textbf{W}}\end{bmatrix}&\textbf{0}\\\
\textbf{0}&\begin{bmatrix}\textbf{F}_{o}\\\ \dot{\textbf{W}}\\\
\ddot{\textbf{W}}\end{bmatrix}\end{bmatrix},\textbf{h}_{j}=\begin{bmatrix}\textbf{x}_{o}+a\textbf{d}_{o,j}\cos\boldsymbol{\alpha}_{o,j}\\\
\textbf{d}_{v,j}\cos\boldsymbol{\alpha}_{v,j}\\\
\textbf{d}_{a,j}\cos\boldsymbol{\alpha}_{a,j}\\\
\textbf{x}_{o}+a\textbf{d}_{o,j}\sin\boldsymbol{\alpha}_{o,j}\\\
\textbf{d}_{v,j}\sin\boldsymbol{\alpha}_{v,j}\\\
\textbf{d}_{a,j}\sin\boldsymbol{\alpha}_{a,j}\\\ \end{bmatrix},$ (21)

$\displaystyle\boldsymbol{\alpha}_{j}=(\boldsymbol{\alpha}_{o,j},\boldsymbol{\alpha}_{a,j},\boldsymbol{\alpha}_{v,j}),\qquad\textbf{d}_{j}=(\textbf{d}_{o,j},\textbf{d}_{v,j},\textbf{d}_{a,j})$

The matrix $\textbf{F}_{o}$ is obtained by stacking the matrix W from (1) as
many times as the number of neighboring vehicles considered for collision
avoidance at a given planning cycle. The vector
$\textbf{x}_{o},\textbf{y}_{o}$ is formed by appropriately stacking
$x_{o,i}(t),y_{o,i}(t)$ at different time instants and for all the neighbors.
Similar construction is followed to obtain
$\boldsymbol{\alpha}_{o},\boldsymbol{\alpha}_{v},\boldsymbol{\alpha}_{a},\textbf{d}_{o},\boldsymbol{d}_{v}\boldsymbol{d}_{a}$.

Constraints (20c)-(20g) acts as substitutes for
$\textbf{g}(\boldsymbol{\xi}_{j})\leq 0$ in the projection operator (line 8,
Alg.1). Please also note the addition of subscript $j$ indicating that the
(20a)-(20g) is defined for the $j^{th}$ sample of ${\boldsymbol{\xi}}_{j}$

The approach of [10] can efficiently handle (20a)-(20d). Thus, our aim is to
accommodate the new constraints (20e)-(20g) while incurring minimal change to
the batch update rule proposed in [10]. This motivates our solution process
that is discussed next.

####  D-A2 Solution Process

The projection process relies on two key ideas. First, we relax the non-convex
equality (20c) and affine inequality constraints as $l_{2}$ penalties and
augment them into the projection cost (20a).

$\mathcal{L}(\boldsymbol{\xi}_{j},\boldsymbol{\lambda}_{j})=\frac{1}{2}\left\|\boldsymbol{\xi}_{j}-\overline{\boldsymbol{\xi}}_{j}\right\|_{2}^{2}-\langle\boldsymbol{\lambda}_{j},{\boldsymbol{\xi}}_{j}\rangle+\frac{\rho}{2}\left\|\textbf{F}{\boldsymbol{\xi}}_{j}-\textbf{h}_{j}\right\|_{2}^{2}+\frac{\rho}{2}\left\|\mathbf{G}\boldsymbol{\xi}_{j}-\textbf{e}(\boldsymbol{\xi}_{j})+\mathbf{s}_{j}\right\|^{2}$
(22)

1:Initialize
${{}^{k}}\boldsymbol{\lambda}_{j},{{}^{k}}\boldsymbol{\xi}_{j},{{}^{k}}\boldsymbol{\alpha}_{o,j},{{}^{k}}\boldsymbol{\alpha}_{v,j},{{}^{k}}\boldsymbol{\alpha}_{a,j}$,
${{}^{k}}\textbf{d}_{o,j}$, ${{}^{k}}\textbf{d}_{v,j}$,
${{}^{k}}\textbf{d}_{a,j}$ for iteration $k=0$

12:for  _$k=1,k\leq\text{maxiter},k++$_ do

2
$\displaystyle{{}^{k+1}}{\boldsymbol{\xi}}_{j}=\arg\min_{{\boldsymbol{\xi}}_{j}}\frac{1}{2}\left\|\boldsymbol{\xi}_{j}-\overline{\boldsymbol{\xi}}_{j}\right\|_{2}^{2}-\langle{{}^{k}}\boldsymbol{\lambda}_{j},{\boldsymbol{\xi}}_{j}\rangle+\frac{\rho}{2}\left\|\textbf{F}{\boldsymbol{\xi}}_{j}-\textbf{h}_{j}({{}^{k}}\boldsymbol{\alpha}_{j},{{}^{k}}\textbf{d}_{j})\right\|_{2}^{2}+\frac{\rho}{2}\left\|\mathbf{G}\boldsymbol{\xi}_{j}-\mathbf{e}({{}^{k}}\boldsymbol{\xi}_{j})+{{}^{k}}\mathbf{s}_{j}\right\|^{2}$
$\displaystyle,\textbf{A}_{eq}{\boldsymbol{\xi}}_{j}=\textbf{b}_{eq}$ (23)

3
$\displaystyle{{}^{k+1}}\boldsymbol{\alpha}_{j},{{}^{k+1}}\textbf{d}_{j}=\arg\min_{\boldsymbol{\alpha}_{j},\textbf{d}_{j}}\left\|\textbf{F}{{}^{k+1}}{\boldsymbol{\xi}}_{j}-\textbf{h}_{j}(\boldsymbol{\alpha}_{j},\textbf{d}_{j})\right\|_{2}^{2}$
(24)

4 $\displaystyle{{}^{k+1}}\textbf{d}_{o,j}=clip(1,\infty)$ (25a)
$\displaystyle{{}^{k+1}}\textbf{d}_{v,j}=clip(\overline{v}_{min},\overline{v}_{max})$
(26a)
$\displaystyle\overline{v}_{min}=\max(v_{min},\sqrt{{{}^{k}}\textbf{d}_{a,j}|\sin({{}^{k+1}}\boldsymbol{\alpha}_{a,j}-{{}^{k+1}}\boldsymbol{\alpha}_{v,j}|}))$
(26b)
$\displaystyle\overline{v}_{max}=\min(v_{max},\sqrt{c_{max}/(|\kappa({{}^{k}}\boldsymbol{\xi}_{j})|(\cos\boldsymbol{{{}^{k+1}}\alpha}_{v,j})^{2})}$
(26c) $\displaystyle{{}^{k+1}}\textbf{d}_{a,j}=clip(0,\overline{a}_{max})$
(27a)
$\displaystyle\overline{a}_{max}=\min\left(a_{max},\frac{{{}^{k+1}}\textbf{d}_{v}^{2}\kappa_{max}}{|\sin(\boldsymbol{{{}^{k+1}}\alpha}_{a,j}-\boldsymbol{{{}^{k+1}}\alpha}_{v,j})|}\right)$
(27b)

5 $\displaystyle{{}^{k+1}}\mathbf{s}=$
$\displaystyle\text{max}\left(0,-\mathbf{G}{{}^{k+1}}\boldsymbol{\xi}_{j}-\mathbf{e}({{}^{k+1}}\boldsymbol{\xi}_{j})\right)$
(28a) $\displaystyle{{}^{k+1}}\boldsymbol{\lambda}=$
$\displaystyle{{}^{k}}\boldsymbol{\lambda}-\frac{\rho}{2}\mathbf{F}^{T}\left(\mathbf{F}{{}^{k+1}}\boldsymbol{\xi}_{j}-\mathbf{h}({{}^{k+1}}\boldsymbol{\alpha}_{j},{{}^{k+1}}\textbf{d}_{j})\right)\frac{\rho}{2}\mathbf{G}^{T}\left(\mathbf{G}{{}^{k+1}}\boldsymbol{\xi}_{j}-\mathbf{e}({{}^{k+1}}\boldsymbol{\xi}_{j})+{{}^{k+1}}\mathbf{s}\right)$
(28b)

6 end for

3:Return ${{}^{k+1}}{\boldsymbol{\xi}}_{j}$

Algorithm 2 Efficient Batch Optimization for Lower Level Trajectory Planning

The constant $\rho$ controls the trade-off between minimizing the projection
cost and constraint residuals. The slack variable
$\textbf{s}_{j}\geq\textbf{0}$ is unknown and will computed by the projection
process along with $\boldsymbol{\xi}_{j}^{*}$ and other variables. The
variable $\boldsymbol{\lambda}_{j}$ is the so-called Lagrange multipliers and
play a crucial role in driving the residuals to zero for any arbitrary $\rho$
[28].

The second part of the idea is to apply Alternating Minimization (AM) approach
for computing the optimal solution of (22) subject to (20c)-(20g). This is
presented in Algorithm 2. At each step, we minimize only one block of
variables while others are fixed at values obtained in the preceding iteration
or the previous step of the current iteration. We elaborate on each step next.

####  D-A3 Analysis Step (23)

At step $k+1$, we use the ${{}^{k}}\boldsymbol{\xi}_{j}$ to make an estimate
of the exact lane bounds $\textbf{e}({{}^{k}}\boldsymbol{\xi}_{j})$ and
curvature $\kappa({{}^{k}}\boldsymbol{\xi}_{j})$. As a result, the curvature
and lane bounds become independent of $\boldsymbol{\xi}_{j}$ and 23 turns to a
equality constrained QP of the form presented in (7). The solution for the
entire batch can computed in one-shot using (8)

####  D-A4 Analysis Step (24)

For a given ${{}^{k+1}}\boldsymbol{\xi}_{j}$, the optimization over
$\boldsymbol{\alpha}_{j},\textbf{d}_{j}$ have a closed form solution that can
be evaluated across all the batch in one-shot [10] . In this step, we ignore
the constraints on $\boldsymbol{\alpha}_{j},\textbf{d}_{j}$.

####  D-A5 Analysis of Step (25a)-(27a)

In these steps, we clip the values of
$\textbf{d}_{o,j},\textbf{d}_{v,j},\textbf{d}_{a,j}$ computed in the previous
step to satisfy their respective bounds. The clipping over $\textbf{d}_{o,j}$
follows from [10] while the remaining two are new additions which are obtained
in the following manner. For a given
${{}^{k+1}}\boldsymbol{\alpha}_{v,j},{{}^{k+1}}\boldsymbol{\alpha}_{a,j}$
obtained in (24) and ${{}^{k}}\textbf{d}_{a,j}$ from previous iteration, the
curvature (20e) and centripetal acceleration (20f) constraints reduce to just
upper and lower bounds on $\textbf{d}_{v,j}$. Similar reasoning leads to
(27a).

####  D-A6 Analysis of Step

The update (28a)-(28b) follows from [29], [28] and herein, we update the
Lagrange multipliers and slack variables based on the residuals at the current
iteration.

Summary: Using a combination of clever reformulations and a AM approach, each
step of the batch projection either boils down to (i) batch QP ((23)) or
simply function evaluations ((24)-(28b) ).


# FlexMap Fusion: Georeferencing and Automated Conflation of HD Maps with
OpenStreetMap

Maximilian Leitenstern1, Florian Sauerbeck1, Dominik Kulmer1, and Johannes
Betz2 The research was partially funded by the Federal Ministry of Education
and Research of Germany (BMBF) within the project Wies’n Shuttle (FKZ
03ZU1105AA) in the MCube cluster, the Bavarian Research Foundation (BFS), and
through basic research funds from the Institute of Automotive
Technology.1Maximilian Leitenstern, Florian Sauerbeck, and Dominik Kulmer are
with the Institute of Automotive Technology, Munich Institute of Robotics and
Machine Intelligence (MIRMI), Technical University of Munich, 85748 Garching,
Germany2Johannes Betz is with the Professorship of Autonomous Vehicle Systems,
Munich Institute of Robotics and Machine Intelligence (MIRMI), Technical
University of Munich, 85748 Garching, GermanyCorresponding author:
<EMAIL_ADDRESS>
###### Abstract

Today’s software stacks for autonomous vehicles rely on HD maps to enable
sufficient localization, accurate path planning, and reliable motion
prediction. Recent developments have resulted in pipelines for the automated
generation of HD maps to reduce manual efforts for creating and updating these
HD maps. We present FlexMap Fusion, a methodology to automatically update and
enhance existing HD vector maps using OpenStreetMap. Our approach is designed
to enable the use of HD maps created from LiDAR and camera data within
Autoware. The pipeline provides different functionalities: It provides the
possibility to georeference both the point cloud map and the vector map using
an RTK-corrected GNSS signal. Moreover, missing semantic attributes can be
conflated from OpenStreetMap into the vector map. Differences between the HD
map and OpenStreetMap are visualized for manual refinement by the user. In
general, our findings indicate that our approach leads to reduced human labor
during HD map generation, increases the scalability of the mapping pipeline,
and improves the completeness and usability of the maps. The methodological
choices may have resulted in limitations that arise especially at complex
street structures, e.g., traffic islands. Therefore, more research is
necessary to create efficient preprocessing algorithms and advancements in the
dynamic adjustment of matching parameters. In order to build upon our work,
our source code is available at https://github.com/TUMFTM/FlexMap_Fusion.

©© 2024 IEEE. Personal use of this material is permitted. Permission from IEEE
must be obtained for all other uses, in any current or future media, including
reprinting/republishing this material for advertising or promotional purposes,
creating new collective works, for resale or redistribution to servers or
lists, or reuse of any copyrighted component of this work in other works.

##  I Introduction

High-definition (HD) maps are essential to today’s autonomous driving systems
[1, 2]. The term HD map describes a geospatial map for autonomous driving that
differs from a conventional, standard-definition (SD) map for human drivers
[3]. SD maps can be seen as interactive paper maps with accuracy from
$5\text{\,}\mathrm{m}10\text{\,}\mathrm{m}$, incorporating interactive
features and the possibility for map updates. Navigation systems utilize these
maps in conjunction with global navigation satellite systems (GNSS) to
determine vehicle positions and calculate routes [4]. However, due to
insufficient accuracy and comprehensiveness for autonomous driving, HD maps
have been developed [2]. In contrast to SD maps, HD maps provide a three-
dimensional representation of the environment with an accuracy of
$10\text{\,}\mathrm{cm}20\text{\,}\mathrm{cm}$ [4]. This includes details like
lane boundaries, traffic signs, and buildings [3, 5]. Therefore, HD maps are
considered an important input for autonomous software stacks, as they are
capable of addressing several limitations of visual sensors, such as
restricted range and susceptibility to occlusion influences.

Figure 1: PCM with original VM (bottom image) and exemplary information from
OSM to be conflated (top image).

Within the field of HD maps, point cloud maps (PCMs) are differentiated from
vector maps (VMs). PCMs represent the three-dimensional shape of objects with
points, typically generated by registering individual scans from Light
Detection and Ranging (LiDAR) sensors [3]. In contrast, VMs describe the
georeferenced position of objects of interest, such as lanes, traffic signs,
or buildings, by points, lines, and polygons [3]. The traditional, manual
annotation of VMs is both costly and time-intensive, leading to scalability
issues for large-scale map creation [5]. Consequently, the automated
generation of large-scale VMs is of high interest. Against this background,
the mapping company HERE [6] is currently developing UniMap, a highly
automated mapping technology aiming to create a unified digital map with
unique timeliness, quality, and spatial coverage [7]. UniMap achieves this by
extracting features from sensor data and combining various map formats, such
as SD maps or HD maps, into a coherent representation of the environment [7].

While the feature extraction and fusion of data from various sensors for HD
map generation is extensively studied in the literature, few references
address the combination of different map formats. To overcome this issue, in
this paper, we present with FlexMap Fusion a modular approach for
postprocessing of existing HD maps that provides the following contributions:

  * •

We present a pipeline for automatic georeferencing of PCMs and VMs based on
manually selectable control points.

  * •

We enable the conflation of semantic attributes from an SD map into a VM.

  * •

We provide a validation and visualization of VMs for facilitated manual
refinement of the user.

  * •

We open-source the pipeline with compatibility to the open-source autonomous
driving software stack Autoware [8]

Figure 1 visualizes the initial data representation of the VM and
OpenStreetMap (OSM)111https://www.openstreetmap.org/ with exemplary attributes
that can be conflated using FlexMap Fusion.

##  II Related Work

In autonomous driving systems, maps address the limitations caused by
conventional sensors [1]. Real-time access to map information requires a
computationally efficient format, leading to the use of HD maps [9]. According
to the Automotive Edge Computing Consortium (AECC) [10] the required accuracy
of a VM is $10\text{\,}\mathrm{cm}20\text{\,}\mathrm{cm}$. One of the main
challenges in HD map creation, alongside completeness and sufficient accuracy,
is to keep the map up-to-date. This requires a modular, scalable framework
ensuring easy expandability and modifiability of map information [1]. The
Autoware software stack is based on the VM format Lanelet2. It was introduced
by Poggenhans et al. [1] in 2018 and uses an XML-based data format with the
file extension .osm that is also used by OSM. The map structure is decomposed
into three layers: The physical layer defines observable objects using points
and linestrings. The relational layer connects physical layer elements to
lanelets, areas, and traffic rules. Lastly, the topological layer provides
contextual information for the relational layer. For data representation,
Lanelet2 contains five so-called primitives:

  * •

Points are the core element defined by 3D position information.

  * •

Linestrings are ordered sequences of points.

  * •

Lanelets are atomic sections for directed motion, e.g. regular lanes and
pedestrian crossings. They are defined by one linestring as left/right
boundary.

  * •

Areas are sections for undirected motion, such as parking areas. They are
defined by one or more linestrings.

  * •

Regulatory elements define traffic rules like speed limits or priority rules.
A lanelet/area refers to a regulatory element if it applies to it.

To store the information in the OSM file, the primitives use attributes
defining, e.g., the street type or the current speed limit.

Current HD map creation processes involve data recording using a Mobile
Mapping System (MMS) with various sensors [5]. Subsequently, the sensor data
undergoes processing, and geometric features are extracted using vehicle-
centric algorithms [11]. Sensor data fusion is studied at various levels to
address the limitations of individual sensors and ensure an accurate map.
Particularly, approaches focusing on VM creation leverage LiDAR and camera
data for map construction [12, 13, 14, 15]. However, a drawback of these is
their limitation to currently extractable information from sensor data. As
several features might not be extractable temporarily from sensor data, e.g.
due to weather conditions, the incorporation of online map data presents a
method for the additional enhancement of VMs. A popular database offering
online map data for free is OSM. Several authors successfully leverage OSM for
vehicle localization [16, 17, 18]. In the context of map construction, Zhou et
al. [19] infer the road network skeleton from OSM to facilitate the
construction of a VM. Maierhofer et al. [20] present a toolbox for the
automatic conversion between OSM, Lanelet2 and various other map formats.
However, our work focuses on combining and validating an existing VM based on
OSM.

The combination of two spatial maps to create an enhanced one is called
conflation and has been extensively studied for SD maps like OSM. It usually
includes three steps: During preprocessing, a rubber-sheet transformation
removes geometric distortions between the datasets [21]. It is followed by
road network matching, which describes the identification of objects in each
dataset corresponding to the same object in reality [22]. Lastly, one object
of each matched pair is transferred to the improved dataset.

Several authors present methodologies for the conflation of OSM with other SD
map formats [23, 24, 25, 22]. The open-source software
Hootenanny222https://github.com/ngageoint/hootenanny provides various features
for the conflation of different features and maps [26]. However, these
approaches have one thing in common: they do not consider HD map formats.

##  III Methodology

The state-of-the-art developments lack a method to efficiently combine VMs
with online map information without much manual effort. We therefore present
FlexMap Fusion as part of the offline mapping pipeline developed for the EDGAR
project at the Technical University of Munich (TUM) [27, 28].

Figure 2: Overview of the presented architecture of FlexMap Fusion.

As an input, a PCM needs to be created from LiDAR sensor data first.
Subsequently, a VM in the Lanelet2 format is created from the PCM, either
manually with
VectorMapBuilder333https://tools.tier4.jp/feature/vector_map_builder_ll2/
(chosen method for this work) or automatically using e.g. the approaches by
Prochazka et al. [29], or Ye et al. [30]. As the map creation process does not
leverage GNSS, the HD map needs to be georeferenced to enable conflation with
OSM data and GNSS usage for localization. Hence, FlexMap Fusion consists of
three single modules: Firstly, within the Map Alignment module, the HD map is
aligned to the real-time kinematic (RTK)-corrected GNSS trajectory of the
vehicle in a projected, local coordinate frame. Secondly, in the Map
Conflation module, available data from OSM is conflated into the VM. Thirdly,
in the Georeferencing module, the projection of local to global coordinates
creates a georeferenced VM. The following sections discuss the implementation
of the single modules of FlexMap Fusion in detail.

###  III-A Map Alignment

The simultaneous localization and mapping (SLAM) process is based only on
LiDAR and leads to a vehicle trajectory (from now on referred to as SLAM
trajectory) and a corresponding PCM in a local coordinate frame with the
initial vehicle position as the origin. The HD map is based on the PCM and
thus in the same coordinate system. To enable conflation with OSM data, the
maps must be referenced globally and geometric distortions from accumulated
errors during the SLAM process must be removed. Belonging to the same vehicle
trajectory in reality, the Map Alignment module leverages the SLAM trajectory
and a RTK-corrected GNSS trajectory to compute the transformations.
GNSS coordinates and OSM nodes are transformed into a grid coordinate system
to facilitate the computation of distances and directions. We use the
universal transverse Mercator (UTM) projection provided by the
Lanelet2444https://github.com/fzi-forschungszentrum-informatik/Lanelet2
library, which is based on the
GeographicLib555https://geographiclib.sourceforge.io/ [31]. To limit the shift
between the two trajectories, the initial GNSS measurement is selected as the
origin for the projection.
Since the orientation of the coordinate system of the SLAM trajectory depends
on the initial vehicle heading, the trajectories may still be randomly twisted
to each other. Hence, we utilize a rigid transformation to correct for the
remaining rotation and potential translation between the trajectories. The
algorithm by Umeyama [32] computes the least-squares estimation between two
sets of points divided into rotation, translation, and scaling. As we do not
want to change Euclidean distances within this step, only the rotational and
translational parts of the resulting transformation are applied. By applying
the transformation to the SLAM trajectory and the HD map, the trajectories and
the maps are aligned in the best way without modifying inner structures.
To remove remaining geometric deviations between the trajectories due to
accumulated errors during SLAM, we use a piecewise linear rubber-sheet
transformation after Griffin and White [33]. Based on a set of so-called
control points (corresponding points in each dataset), it applies multiple
linear rigid transformations to subdivided regions of the map, specifically
triangles. As the transformation matrices of the triangles are derived from
the control points defining its corners, only the boundaries are flexed while
the transformation stays linear within the triangle [33]. For a detailed
explanation on the functionality of a rubber-sheet transformation, the reader
is referred to [33]. The algorithm is applied to the GNSS and SLAM trajectory
of the previous step. At the beginning, the user is prompted to manually
select a predetermined number of control points on the two trajectories,
showing a high likelihood of correspondence. After the creation of rectangles
around the trajectories and their division into two initial triangles, the
triangulation is computed. For each control point, the corresponding triangle
is determined and replaced by three new triangles. To avoid the creation of
long, narrow triangles that may diminish the transformation result, a
quadrilateral test is performed on newly created triangles and their
neighbors. Subsequently, the single transformation matrices are computed based
on the vertices of the defining triangles:

$\vec{v_{k}}=\vec{T_{j}}\,\vec{u_{k}};\quad\vec{v_{l}}=\vec{T_{j}}\,\vec{u_{l}};\quad\vec{v_{m}}=\vec{T_{j}}\,\vec{u_{m}};$
(1)

In this context, $\vec{T_{j}}$ represents the transformation matrix for a
triangle $j$, and $\vec{v_{i}},\,\vec{u_{i}}$ with $i\in\\{k,\,l,\,m\\}$
denote its vertices on the GNSS and SLAM trajectory, respectively. Expanding
the equation for an individual vertex yields:

$\begin{pmatrix}v_{ix}\\\ v_{iy}\\\
1\end{pmatrix}=\begin{pmatrix}t_{11}&t_{12}&t_{13}\\\ t_{21}&t_{22}&t_{23}\\\
t_{31}&t_{32}&t_{33}\end{pmatrix}\,\begin{pmatrix}u_{ix}\\\ u_{iy}\\\
1\end{pmatrix}\quad\quad i\in\\{k,\,l,\,m\\}.$ (2)

Forming this equation for each vertex results in a total of nine equations,
enabling the determination of the nine unknown components of $\vec{T_{j}}$.
The transformation is then applied by identifying the surrounding triangle
$\vec{T_{j}}$ of each point $\vec{x}$ and computing

$\vec{x}^{\prime}=\vec{T}_{j}\,\vec{x}.$ (3)

Table I summarizes the input parameters of FlexMap Fusion with their initial
reference system and the applied transformations within the Map Alignment
module. As existing approaches for rubber-sheet transformations only work in
2D, the height information of the HD map is currently not adjusted during the
Map Alignment process.

TABLE I: Overview of Map Alignment process with reference system and applied transforms/projections of inputs. Property/ Parameter |  GNSS trajectory |  SLAM trajectory |  PCM/VM |  OSM
---|---|---|---|---
Reference system |  global |  local |  local |  global
UTM projection |  ✓ |  X |  X |  ✓
Rigid transform |  X |  ✓ |  ✓ |  X
Rubber-sheet transform |  X |  ✓ |  ✓ |  X

###  III-B Map Conflation

Following the Map Alignment module, the HD and OSM map are geometrically
aligned with small deviations. However, due to the SD map format of OSM, it
depicts the road network at a different level of detail (LoD) compared to
Lanelet2 (3(a)). Hence, we apply a preprocessing step to the lanelet map as
shown in 3(b).

: Lanelet2 : OSM

(a) intersection representation

: Lanelet2 collapsed

(b) preprocessed Lanelet2

Figure 3: Preprocessing of Lanelet2 map.

Following the approach of Zhang [22] for matching of dual carriageways,
adjacent lanelets are collapsed to a single centerline. A distinction is made
between adjacent lanes in the same direction (forward lanes) and adjacent
lanes in the opposite direction (backward lanes). The identification numbers
of the individual lanelets that a centerline represents are appended to them
as attributes. Connecting centerlines based on the connectivity of the
lanelets maintains topological consistency to the original map.
For the identification of corresponding objects in the VM and OSM, a road
network matching algorithm is implemented. Following the ability to handle
different road network resolutions with high matching performance but
simultaneously low implementation and parametrization effort, we chose a
Buffer Growing algorithm based on Zhang and Meng [34]: First, a reference
polyline is created by connecting single centerline segments of the
preprocessed lanelet map (e.g. segments $A-B$ in Figure 4).

: Lanelet2 collapsed  : OSM

Figure 4: Principle of Buffer Growing algorithm.

After that, buffers are created around the single segments and all elements
from OSM that fall inside them are identified as match candidates (segments
$F-G$, $C-D$, $C-E$, and $D-E$ in Figure 4). If no match candidates could be
found, the buffer size is iteratively increased three times. If multiple match
candidates are found, the correct match is selected by computing a similarity
score between the reference and OSM polyline. The score forms a weighted sum
of the angle and distance between a polyline’s starting and ending point, its
length, and the area of the enclosed polygon. While each segment of the
preprocessed lanelet map is only used once during the matching process, OSM
segments may be matched multiple times. This addresses sections where the LoD
between the lanelet map and OSM differs despite the preprocessing step, e.g.
at an intersection (Figure 3). Details on the implementation of the matching
algorithm can be found in the work of Zhang and Meng [34]. In contrast to
their approach, we do not use self-learning characteristics to improve
matching accuracy.
Once corresponding objects are identified in the preprocessed lanelet map and
the OSM street network, the available information can be transferred to the
lanelet map. As OSM does not contain lane-level details, e.g. the position of
road boundaries, direct conflation of positional information from OSM to the
lanelet map is not possible. Additionally, the positional accuracy and,
notably, the reliability of the OSM road network do not meet the requirements
of a lanelet map. Nevertheless, the rich amount of semantic attributes in OSM
can be utilized to enhance the lanelet map. Hence, conflation is carried out
in three steps: First, semantic attributes that may not be extractable from
LiDAR or camera information are transferred. The attribute highway is mapped
onto the Lanelet2 attributes subtype and location using a custom mapping based
on the OSM documentation [35]. Figure 5 visualizes all implemented
correspondences for the Attribute Transfer.

highwaysubtypelocationmaxspeedspeed_limitnameroad_namesurfaceroad_surfaceonewayone_waylane_markingslane_markings
Figure 5: Attribute transfer correspondences between OSM (orange) and Lanelet2
(blue).

Although there is no counterpart in Lanelet2, the OSM tag lane_markings is
transferred as is to the lanelet map, as it provides beneficial information
for the vehicle when using the map. Figure 6 visualizes the realization of the
attribute transfer based on the matching results.

: Lanelet2 : OSM  : Lanelet2 collapsed  : Match links

Figure 6: Principle of attribute transfer.

Next to the transfer of attributes, the tag lanes in OSM is used to validate
the correct amount of adjacent lanelets in the lanelet map. Within the
visualization of FlexMap Fusion, the adjacent lanelets are colored green if
the numbers match, red if they differ, and blue if the lanes tag is missing in
OSM. Lastly, FlexMap Fusion can automatically remove mistakenly mapped lanelet
fragments from the semantic mapping process (e.g. due to high reflections of
objects next to the road). A single lanelet may be deleted if it has neither a
predecessor nor a follower (i.e. fragment) and the number of adjacent lanelets
at this point exceeds the indicated one from the lanes tag in OSM.

###  III-C Georeferencing

During the Map Alignment process, the SLAM trajectory and the PCM/VM are
aligned to the projected GNSS trajectory. Therefore, an inverse application of
the UTM projection used in the Map Alignment module enables their
georeferencing.

##  IV Results

Experimental results of FlexMap Fusion are presented using self-recorded data
from the EDGAR research vehicle [28] in Garching, Germany. The initial VMs in
this work were created manually as described in section II.
Figure 7 shows the results of the Map Alignment process for a route of EDGAR
around the TUM campus in Garching. Despite the Umeyama transformation,
deviations from the SLAM trajectory to the projected GNSS trajectory remain
(7(a)). These are removed by the rubber-sheet transformation with manually
selected control points (orange stars). As the two trajectories differ in
size, the quantitative results are calculated by the minimum distance of a
point on the SLAM trajectory to the GNSS trajectory. For the given route and
selection of control points, a mean deviation of $1.08\text{\,}\mathrm{m}$, a
standard deviation of $1.61\text{\,}\mathrm{m}$, and a root mean squared error
(RMSE) of $1.93\text{\,}\mathrm{m}$ was achieved. Following the selection of
control points, the section with missing GNSS reception (at $X\thickapprox 0$,
$Y\thickapprox 100$) does not affect the results negatively 7(b).

(a) Triangulation

(b) Final alignment

Figure 7: Final results of the Map Alignment process applied to an example
route. For triangulation, $10\text{\,}$ control points are selected manually
(orange stars and green triangles). The colored line represents the SLAM
trajectory.

The matching algorithm creates a set of reference polylines from the
preprocessed lanelet map and searches for matching polylines of OSM segments.
This enables a classification into matched and unmatched reference polylines.
These categories are further separated by predictive analytics [36]:

  * •

True Positive
Correct identification of match.

  * •

False Positive
Wrong identification of existing match (wrong OSM polyline or no match).

  * •

True Negative
Correct prediction that there is no match.

  * •

False Negative
No identification of an existing match.

While the distinction between True and False Positives is based on the
geometric similarity score [34], True and False Negatives need to be
identified manually. This allows the computation of precision and recall after
[36]. For the given route in Figure 7, our matching algorithm identifies a
match for $85.8\text{\,}\mathrm{\char 37\relax}$ of the reference polylines
with a length above $1.5\text{\,}\mathrm{m}$ (the threshold is introduced as
short polyline fragments are an unavoidable consequence of the preprocessing
step). This leads to a precision of $68.72\text{\,}\mathrm{\char 37\relax}$
and a recall of $63.04\text{\,}\mathrm{\char 37\relax}$. Figure 8 visualizes
the matching results for an exemplary intersection.

Figure 8: Exemplary matching results for intersection. Green arrows indicate
linkages from OSM polylines to reference polylines from the preprocessing
step.

The algorithm proves to reliably match the created reference polylines to OSM
segments despite remaining differences in data representation. Limitations of
the matching algorithm are sections with strong differences in road network
representation, e.g. at traffic islands that are not present in OSM. Although
this may be resolved by increasing the buffer size, it would also increase the
amount of False Positive matches.
Finally, the georeferenced lanelet map is to be evaluated. As there is no
ground truth data for the position of the road network, the results can only
be evaluated qualitatively using orthophotos by
OPENDATA666https://geodaten.bayern.de/opengeodata/. Figure 9 shows the results
for the top right section of the route from Figure 7.

Figure 9: Georeferenced VM and GNSS trajectory over orthophoto of OPENDATA.

A visual comparison indicates a strong overlap between the lanelet map and the
orthophoto. Within this section, the mean deviation between the SLAM and the
GNSS trajectory is $0.38\text{\,}\mathrm{m}$ with a standard deviation of
$0.27\text{\,}\mathrm{m}$ and an RMSE of $0.46\text{\,}\mathrm{m}$.

##  V Discussion

The presented results prove the concept of FlexMap Fusion. Our pipeline
features a modular structure with strong and reliable algorithms. However,
there are still shortcomings in the current implementation that motivate
future work. As the control points are selected manually, the results of Map
Alignment and thus, Conflation and Georeferencing are not directly
reproducible. Furthermore, the user needs reference points for control point
selection, e.g. corner apexes. Therefore, deviations between the trajectories
increase for sections with few intersections/corners. Also, it needs to be
noted that the Map Alignment and Georeferencing processes require a reliable
GNSS signal. Taking into account GNSS accuracy and the deviations between the
trajectories, our results do not yet fulfill the accuracy requirements of
$10\text{\,}\mathrm{cm}20\text{\,}\mathrm{cm}$ of a VM. However, the accuracy
may be increased by automatically selecting control points (e.g. based on
corresponding timestamps of GNSS and SLAM poses).
Considering the matching algorithm, problems arise at complex structures where
the differences in the LoD of OSM and the preprocessed VM become apparent
(e.g. traffic islands that may not be represented in OSM). These problems
could be resolved by improving the preprocessing step or a dynamic adjustment
of the geometric similarity thresholds (e.g. based on the length of the
reference polyline) of the Buffer Growing algorithm. However, it is noted that
the evaluation of the matching results may be ambiguous for individual cases.
Therefore, additional focus needs to be spent on a clear definition of the
four categories defined in section IV. Regarding the conflation process
itself, larger studies are necessary to evaluate the accuracy and reliability
of OSM data. Although the considered area around Garching, Germany showed good
coverage in OSM, several sections with missing tags or wrong/ambiguous data
were detected. Therefore, a strategy needs to be developed for handling wrong
data in OSM. The last point to be mentioned is timeliness: So far, the tool
has only been tested with both the VM and OSM being up-to-date. Hence, future
work on FlexMap Fusion needs to include different temporal versions of map
data to account for the case of either one or both maps being outdated.

##  VI Conclusion and Future Work

With FlexMap Fusion, we present a novel approach to enable georeferencing of
existing HD maps based on an RTK-corrected GNSS trajectory. Furthermore, our
pipeline can automatically enhance and validate VMs using OSM. Although this
open-source tool still features limitations, it reduces human labor during HD
map generation and thus increases the scalability of the mapping pipeline.
Following the exponential growth of users of OSM and thus its reliability and
accuracy, work is ongoing on the various approaches to improving the pipeline.

## References

  * [1] F. Poggenhans, J.-H. Pauls, J. Janosovits, S. Orf, M. Naumann, F. Kuhnt, and M. Mayr, “Lanelet2: A high-definition map framework for the future of automated driving,” in _2018 21st International Conference on Intelligent Transportation Systems (ITSC)_ , 2018, pp. 1672–1679.
  * [2] H. G. Seif and X. Hu, “Autonomous driving in the icity—hd maps as a key challenge of the automotive industry,” _Engineering_ , vol. 2, no. 2, pp. 159–162, 2016.
  * [3] J. Jeong, J. Y. Yoon, H. Lee, H. Darweesh, and W. Sung, “Tutorial on high-definition map generation for automated driving in urban environments,” _Sensors_ , vol. 22, no. 18, 2022.
  * [4] R. Liu, J. Wang, and B. Zhang, “High definition map for automated driving: Overview and analysis,” _The Journal of Navigation_ , vol. 73, no. 2, pp. 324–341, 2020.
  * [5] Z. Bao, S. Hossain, H. Lang, and X. Lin, “High-definition map generation technologies for autonomous driving,” 2022.
  * [6] HERE. Here hd live map. [Online]. Available: https://www.here.com/platform/HD-live-map
  * [7] A. James, “Here introduces unimap to speed up the creation of digital maps,” _ADAS & Autonomous Vehicle International_, 2023. [Online]. Available: https://www.autonomousvehicleinternational.com/news/here-introduces-unimap-to-speed-the-creation-of-digital-maps.html
  * [8] S. Kato, S. Tokunaga, Y. Maruyama, S. Maeda, M. Hirabayashi, Y. Kitsukawa, A. Monrroy, T. Ando, Y. Fujii, and T. Azumi, “Autoware on board: Enabling autonomous vehicles with embedded systems,” in _2018 ACM/IEEE 9th International Conference on Cyber-Physical Systems (ICCPS)_ , 2018, pp. 287–296.
  * [9] A. Khoche, M. K. Wozniak, D. Duberg, and P. Jensfelt, “Semantic 3d grid maps for autonomous driving,” in _2022 IEEE 25th International Conference on Intelligent Transportation Systems (ITSC)_ , 2022, pp. 2681–2688.
  * [10] Automotive Edge Computing Consortium, “Operational behavior of a high definition map application,” 2020. [Online]. Available: https://aecc.org/wp-content/uploads/2020/07/Operational_Behavior_of_a_High_Definition_Map_Application.pdf
  * [11] L. Zheng, B. Li, B. Yang, H. Song, and Z. Lu, “Lane-level road network generation techniques for lane-level maps of autonomous vehicles: A survey,” _Sustainability_ , vol. 11, no. 16, 2019.
  * [12] Q. Li, Y. Wang, Y. Wang, and H. Zhao, “Hdmapnet: An online hd map construction and evaluation framework,” in _2022 International Conference on Robotics and Automation (ICRA)_ , 2022, pp. 4628–4634.
  * [13] Y. Liu, Y. Tuan, Y. Wang, Y. Wang, and H. Zhao, “Vectormapnet: End-to-end vectorized hd map learning,” 2022.
  * [14] H. Dong, X. Zhang, X. Jiang, J. Zhang, J. Xu, R. Ai, W. Gu, H. Lu, J. Kannala, and X. Chen, “Superfusion: Multilevel lidar-camera fusion for long-range hd map generation and prediction,” 2022.
  * [15] B. Liao, S. Chen, Y. Zhang, B. Jiang, Q. Zhang, W. Liu, C. Huang, and X. Wang, “Maptrv2: An end-to-end framework for online vectorized hd map construction,” 2023.
  * [16] Y. Cho, G. Kim, S. Lee, and J.-H. Ryu, “Openstreetmap-based lidar global localization in urban environment without a prior lidar map,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4999–5006, 2022.
  * [17] G. Floros, B. van der Zander, and B. Leibe, “Openstreetslam: Global vehicle localization using openstreetmaps,” in _2013 IEEE International Conference on Robotics and Automation_ , 2013, pp. 1054–1059.
  * [18] M. Ismail, A. Tarek, P. M. Plaza, D. Martin Gomez, J. M. Armingol, and M. Abdelaziz, “Advanced mapping and localization for autonomous vehicles using osm,” in _2019 IEEE International Conference on Vehicular Electronics and Safety (ICVES)_ , 2019, pp. 1–6.
  * [19] Y. Zhou, Y. Takeda, M. Tomizuka, and W. Zhan, “Automatic construction of lane-level hd maps for urban scenes,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2021, pp. 6649–6656.
  * [20] M. K. Sebastian Maierhofer and M. Althoff, “Commonroad scenario designer: An open-source toolbox for map conversion and scenario creation for autonomous vehicles,” in _Proc. of the IEEE Int. Conf. on Intelligent Transportation Systems_ , 2021, pp. 3176–3182.
  * [21] M. A. Cobb, M. J. Chung, H. Foley III, F. E. Petry, K. B. Shaw, and H. V. Miller, “A rule-based approach for the conflation of attributed vector data,” _Geoinformatica_ , vol. 2, no. 1, pp. 7–35, 1998.
  * [22] M. Zhang, “Methods and implementations of road-network matching,” PhD Thesis, Technical University of Munich, Munich, 2009. [Online]. Available: https://mediatum.ub.tum.de/doc/820125/820125.pdf
  * [23] G. Agarwal, L. V. Lakshmanan, X. Gao, K. Ventullo, S. Mohapatra, and S. Basu, “Mayur: Map conflation using early pruning and rank join,” in _Proceedings of the 29th International Conference on Advances in Geographic Information Systems_ , ser. SIGSPATIAL ’21. New York, NY, USA: Association for Computing Machinery, 2021, pp. 550–553.
  * [24] T. L. Lei, “Geospatial data conflation: a formal approach based on optimization and relational databases,” _International Journal of Geographical Information Science_ , vol. 34, no. 11, pp. 2296–2334, 2020.
  * [25] M. Hacar and T. Gökgöz, “A new, score-based multi-stage matching approach for road network conflation in different road patterns,” _ISPRS International Journal of Geo-Information_ , vol. 8, no. 2, 2019.
  * [26] R. Canavosio-Zuzelski, J. Surratt, D. Bower, J. Governski, and M. Sorenson, “Hootenanny: web enabeled geospatial vector-data conflation and map generation,” in _ASPRS 2015 annual conference_ , 2015, pp. 4–8.
  * [27] F. Sauerbeck, D. Kulmer, M. Pielmeier, M. Leitenstern, C. Weiß, and J. Betz, “Multi-lidar localization and mapping pipeline for urban autonomous driving,” in _2023 IEEE SENSORS_ , 2023, pp. 1–4.
  * [28] P. Karle, T. Betz, M. Bosk, F. Fent, N. Gehrke, M. Geisslinger, L. Gressenbuch, P. Hafemann, S. Huber, M. Hübner, S. Huch, G. Kaljavesi, T. Kerbl, D. Kulmer, T. Mascetta, S. Maierhofer, F. Pfab, F. Rezabek, E. Rivera, S. Sagmeister, L. Seidlitz, F. Sauerbeck, I. Tahiraj, R. Trauth, N. Uhlemann, G. Würsching, B. Zarrouki, M. Althoff, J. Betz, K. Bengler, G. Carle, F. Diermeyer, J. Ott, and M. Lienkamp, “Edgar: An autonomous driving research platform – from feature development to real-world application,” 2023.
  * [29] J. P. David Prochazka and J. Landa, “Automatic lane marking extraction from point cloud into polygon map layer,” _European Journal of Remote Sensing_ , vol. 52, no. sup1, pp. 26–39, 2019.
  * [30] C. Ye, H. Zhao, L. Ma, H. Jiang, H. Li, R. Wang, M. A. Chapman, J. M. Junior, and J. Li, “Robust lane extraction from mls point clouds towards hd maps especially in curve road,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 2, pp. 1505–1518, 2022.
  * [31] J. W. Hager, J. F. Behensky, B. W. Drew, and D. M. A. H. C. W. DC, “The universal grids: Universal transverse mercator (utm) and universal polar stereographic (ups). edition 1,” _DEFENSE MAPPING AGENCY HYDROGRAPHIC/TOPOGRAPHIC CENTER WASHINGTON DC, Tech. Rep_ , 1989.
  * [32] S. Umeyama, “Least-squares estimation of transformation parameters between two point patterns,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , vol. 13, no. 4, pp. 376–380, 1991.
  * [33] Marvin S. White, Jr. and Patricia Griffin, “Piecewise linear rubber-sheet map transformation,” _The American Cartographer_ , vol. 12, no. 2, pp. 123–131, 1985.
  * [34] M. Zhang, W. Shi, and L. Meng, “A generic matching algorithm for line networks of different resolutions,” in _Workshop of ICA commission on generalization and multiple representation_ , vol. 9, 2005, pp. 101–110.
  * [35] Openstreetmap wiki. [Online]. Available: https://wiki.openstreetmap.org/wiki/Main˙Page
  * [36] A. Hackelöer, “Conflation of road networks from digital maps,” Ph.D. dissertation, Technical University of Munich, Munich, 2016.