# VDB-GPDF:
Online Gaussian Process Distance Field with VDB Structure

Lan Wu1, Cedric Le Gentil1, and Teresa Vidal-Calleja1 1All authors are with
the Robotics Institute, Faculty of Engineering and IT, University of
Technology Sydney, Ultimo, NSW 2007, Australia<EMAIL_ADDRESS>This work
was supported by ARIA Research and the Australian Government via the
Department of Industry, Science, and Resources CRC-P program (CRCPXI000007)
and the Australian Research Council Discovery Project under Grant
DP210101336.This work has been submitted to the IEEE for possible publication.
Copyright may be transferred without notice, after which this version may no
longer be accessible.

###### Abstract

Robots reason about the environment through dedicated representations. Popular
choices for dense representations exploit Truncated Signed Distance Functions
(TSDF) and Octree data structures. However, TSDF is a projective signed
distance obtained directly from depth measurements that overestimates the
Euclidean distance. Octrees, despite being memory efficient, require tree
traversal and can lead to increased runtime in large scenarios. Other
representations based on Gaussian Process (GP) distance fields are appealing
due to their probabilistic and continuous nature, but the computational
complexity is a concern. In this paper, we present an online efficient mapping
framework that seamlessly couples GP distance fields and the fast-access VDB
data structure. This framework incrementally builds the Euclidean distance
field and fuses other surface properties, like intensity or colour, into a
global scene representation that can cater for large-scale scenarios. The key
aspect is a latent Local GP Signed Distance Field (L-GPDF) contained in a
local VDB structure that allows fast queries of the Euclidean distance,
surface properties and their uncertainties for arbitrary points in the field
of view. Probabilistic fusion is then performed by merging the inferred values
of these points into a global VDB structure that is efficiently maintained
over time. After fusion, the surface mesh is recovered, and a global GP Signed
Distance Field (G-GPDF) is generated and made available for downstream
applications to query accurate distance and gradients. A comparison with the
state-of-the-art frameworks shows superior efficiency and accuracy of the
inferred distance field and comparable reconstruction performance. The
accompanying code will be publicly available333https://github.com/UTS-
RI/VDB_GPDF.

Keywords: Gaussian Process Distance Field, OpenVDB, Euclidean Distance Field,
Gradient Field, Mapping.

(a)

(b)

(c)

(d)

(d)

Figure 1: 3D dense reconstruction and distance field from VDB-GPDF framework.
a) Shows the incrementally built mesh coloured with the fused LiDAR intensity.
Zooming in to visualise b) the ceiling inside the corridor and c) the stairs
and windows around the corner of the quad. d) Shows a horizontal slice of the
inferred distance field 0.9m above the ground.

##  I Introduction

Robots understand and interact with the world in a meaningful and efficient
manner via effective representations. Building a representation that underpins
perception, control, navigation, learning, and manipulation is fundamental but
has key requirements that include: 1) high accuracy to reflect the true nature
of the unknown environment, 2) efficiency to allow online performance, 3)
scalability to handle large-scale scenarios, 4) adaptability to deal with
dynamic changes, 5) compatibility with varying sensors, 6) flexibility to
provide the needed output space, 7) robustness to ensure reliable operation in
the presence of noise and 8) being dense and potentially generative to deal
with incomplete and discrete data.

Recent frameworks based on distance field representations that fulfil some of
the above-mentioned requirements have been proposed in the robotics literature
[1, 2, 3, 4, 5]. In general, these frameworks are dense and efficient enough
to run online, can deal up to a certain point with dynamic objects and are
catered to any depth sensor. Most of them incrementally build a projective [1,
3] or non-projective [5] Truncated Signed Distance Field (TSDF) or occupancy
map [2, 4] and some can then recover an approximation of the Euclidean Signed
Distance Field (ESDF) [1, 2, 5, 4]. Despite being dense volumetric
representations, these frameworks do not rely on generative models thus the
representation is maintained at the resolution that was built and only
contains information where the data was observed.

On the other hand, Gaussian Process (GP)-based framework proposed in our
previous work [6, 7, 8] are probabilistic generative models that are capable
of building accurate Euclidean Distance Field (EDF)s. GP-based frameworks,
however, suffer from high computational complexity. To solve this issue and
allow online incremental mapping works like [9, 8, 10] use the Octree data
structure. The time of accessing the Octree grows with the number of nodes,
thus suffering from scalability issues for large datasets. As a fast-access
data structure, OpenVDB was used recently proposed in VDBFusion [3]. VDBFusion
is a very efficient incremental reconstruction framework based on TSDF, which
does not recover the EDF.

In this paper, we propose a framework that couples the OpenVDB data structure
with GP distance fields. The so-called VDB-GPDF framework builds a scene
representation that aims to meet all the key requirements mentioned above. It
offers the accuracy and scalability to build a global and incremental scene
representation that can cater for room-sized indoor scenes and large-scale
outdoor datasets and is compatible with depth sensing such as RGB-D cameras or
LiDARs. VDB-GPDF deals implicitly with dynamic changes, handles
probabilistically noisy measurements, and has the ability to complete data at
the required resolution. Our framework can provide multiple outputs such as
accurate Euclidean distance, its gradients, surface properties like colour,
intensity or instance segmentation, and a dense mesh with informative
textures. The key aspect of this work is the local VDB structure that contains
a Local GP Signed Distance Field (L-GPDF), a temporary latent model that
enables queries at arbitrary testing points on the current field of view.
Followed by a probabilistic fusion that merges the inferred values of the
testing points into a global VDB. The contributions of this paper are as
follows:

  * •

A seamless coupling of the GPDF with VDB data structure to enable online
incremental performance for large-scale mapping.

  * •

A probabilistic fusion that given the current measurements fits a local GPDF,
which allows us to fuse using its mean and uncertainty with the global ESDF
and other surface properties.

  * •

A generative model that efficiently produces the ESDF via a Global GPDF
trained based on the estimated surface which enables downstream applications
to query accurate distance and gradients.

We evaluate our proposed VDB-GPDF against the state-of-the-art frameworks
showing comparable reconstruction performance and superior efficiency and
accuracy of the inferred distance field.

##  II Related Work

Efficient dense volumetric mapping is appealing for the ability to
incrementally build a representation of an unseen environment that can be used
for localisation, visualisation, navigation and manipulation in particular
when the output space is an ESDF. Inspired by [11], frameworks like Voxblox
[1] and Voxfield [12] propose solutions to compute the ESDF as a post-
processing step from the projective or non-projective TSDF respectively
through wavefront propagation using BFS (Breadth-First Search) algorithm.
Using occupancy instead of TSDF, FIESTA [2] also obtains the ESDF from the
very efficiently built occupancy map using multiple customised data
structures. Employing a less customised data structure, VDB-EDT adopts OpenVDB
[13, 14] for occupancy mapping and a distance transform function to represent
the EDF hierarchically [4]. Other than occupancy mapping, VDBFusion [3]
combines OpenVDB with TSDF fusion to generate volumetric dense maps
efficiently. However, VDBFusion does not generate the ESDF as an output.
VDBblox [5] adopts the OpenVDB structure to maintain high efficiency. However,
VDBblox continues to use TSDF fusion and then propagates it to approximate the
ESDF as in [12].

Instead of TSDF or occupancy fusion as all these approaches, we propose to
fuse directly on ESDF using an L-GPDF, which allows us to query the Euclidean
distance with uncertainty for any point in the field of view. The fusion is
then done by merging the inferred EDSF values into the global OpenVDB
structure. In Table I, we summarise the most popular online dense volumetric
mapping frameworks and add the proposed framework to highlight its difference
and novelty.

TABLE I: Comparison of online dense mapping frameworks Framework | Data Structure | Fusion | Mesh | ESDF
---|---|---|---|---
VDBFusion [3] | OpenVDB | TSDF | ✓ | ✗
VDB-EDT [4] | OpenVDB | Occupancy | ✗ | ✓
VDBblox [5] | OpenVDB | TSDF | ✓ | ✓
Voxblox [1] | Hash Map | TSDF | ✓ | ✓
Voxfield [12] | Hash Map | TSDF | ✓ | ✓
FIESTA [2] | Multiple | Occupancy | ✗ | ✓
VDB-GPDF (ours) | OpenVDB | ESDF | ✓ | ✓

GP-based distance fields have been proposed as a probabilistic and generative
model to represent complex environments [9, 6]. In our prior work [6, 7], we
proposed a theoretical sound formulation to estimate the Euclidean distance
field by applying the logarithmic transformation to a GP formulation. To
improve the accuracy of the distance estimate, we later introduced the
reverting GP distance field [8]. Although GP-based frameworks are
computationally intensive, Octree structures have enabled fast computations in
[9, 8] and more recently in [10] for online incremental mapping. However,
Octrees expand the nodes overtime and require tree traversal resulting in a
less scalable data structure for large scenarios. Recently, methods like the
Gaussian surface model or mixture model [15] have been introduced to model an
EDF using fused point clouds or batch mapping, but a substantial gap remains
in achieving incremental performance. Via the VDB data structure, our proposed
approach facilitates efficiency and scalability while maintaining the
advantages of the GPDFs.

Beyond GPs, other generative methods for dense volumetric mapping have
recently been proposed in the literature. A large number of works have
examined the potential of deep learning techniques for EDF representations.
Inspired by [16], the work in [17] proposes using an implicit neural
representation with Eikonal regularisation to approximate the ESDF for points
distant from the surface. The work in [18] introduced learning an EDF
approximation from Neural Radiance Fields (NeRFs) using occupancy. Similarly,
inspired from [17], iSDF [19] is proposed to use a neural signed distance
field for mapping. To achieve efficient performance in the learning approach,
DI-Fusion [20] proposes an online incremental SDF using clustering blocks, and
HIO-SDF [21] applies hierarchical data structure. Despite the advantages of
this type of continuous representation with low memory consumption, all these
approaches need extensive pre-training to enable incremental mapping and rely
on high-performance GPU capabilities.

##  III Preliminaries

###  III-A Gaussian Process Distance Field

GP [22] is a non-parametric regression approach that models a distribution
over functions. We use the so-called GP Distance Field method originally
presented in [8] to model our distance field. Consider a surface $\mathcal{S}$
in a Euclidean space $\mathbb{R}^{D}$, and a set of discrete observations of
$S$ as $\mathbf{y}=\left\\{y_{j}\right\\}_{j=1}^{J}\in\mathbb{R}$ taken at
locations $\mathbf{X}=\\{\mathbf{x}_{j}\\}_{j=1}^{J}\in\mathbb{R}^{D}$. By
modelling the latent scalar field which can be interpreted as an occupancy
field $o(\mathbf{x}):\mathbb{R}^{D}\mapsto\mathbb{R}$ of the space with a GP
as $o\sim\mathcal{GP}\left(0,k(\mathbf{x},\mathbf{x}^{\prime})\right)$, it is
possible to infer $\hat{o}(\mathbf{x}_{*})$ at any location in the space. Let
us arbitrarily define the occupied area to be equal to 1. Therefore,
$\mathbf{y}$ is equal to $\mathbf{1}$ for the GP latent inference

$\displaystyle\hat{o}(\mathbf{x}_{*})$
$\displaystyle={\mathbf{k}_{\mathbf{x}_{*}\mathbf{X}}}\left({\mathbf{K}_{\mathbf{X}\mathbf{X}}}+\sigma_{o}^{2}\mathbf{I}\right)^{-1}\mathbf{1}.$
(1)

where $\sigma_{o}^{2}$ is the variance of noise. The uncertainty is then
inferred using

$\displaystyle\hat{u}(\mathbf{x}_{*})$
$\displaystyle={\mathbf{k}_{\mathbf{x}_{*}\mathbf{x}_{*}}}+{\mathbf{k}_{\mathbf{x}_{*}\mathbf{X}}}\left({\mathbf{K}_{\mathbf{X}\mathbf{X}}}+\sigma_{o}^{2}\mathbf{I}\right)^{-1}{\mathbf{k}_{\mathbf{X}\mathbf{x}_{*}}}.$
(2)

The distance field ${\hat{d}}(\mathbf{x}_{*})$ given any location
$\mathbf{x}_{*}$ is obtained by applying the _reverting_ function $r$ to the
latent field as

$\displaystyle{\hat{d}}(\mathbf{x}_{*})=r\left(\hat{o}\left(\mathbf{x}_{*}\right)\right).$
(3)

Considering the square exponential kernel
${{k}\left(\mathbf{x},\mathbf{x}^{\prime}\right)}=\sigma^{2}\exp\left(-\frac{\lVert\mathbf{x}-\mathbf{x}^{\prime}\rVert{}^{2}}{2l^{2}}\right)$,
substituting the reverting function of square exponential kernel into Eq. 3
gives us the distance inference at $\mathbf{x}_{*}$,

$\displaystyle{\hat{d}}(\mathbf{x}_{*})=\sqrt{-2l^{2}\log\left(\frac{\hat{o}\left(\mathbf{x}_{*}\right)}{\sigma^{2}}\right)}.$
(4)

and variance as

$\hat{v}(\mathbf{x}_{*})=\Lambda\hat{u}(\mathbf{x}_{*})\Lambda^{\top}.$ (5)

where $\Lambda$ is equal to
${l\sigma_{o}^{2}}/({\hat{o}\left(\mathbf{x}_{*}\right)\sqrt{2\log\left({\hat{o}\left(\mathbf{x}_{*}\right)}/{\sigma_{o}^{2}}\right)}})$.
As pointed out in [8], the variance of the latent field is propagated through
the reverting function using its Jacobian with the linearisation point at the
surface. Faraway from the surface, the uncertainty of the estimated distance
field grows rapidly. Therefore, the variance is only informative around the
surface.

Furthermore, to estimate the gradient of the distance field, we simply apply
the differentiation (linear) operator as

$\displaystyle\nabla\hat{d}\left(\mathbf{x}_{*}\right)=\nabla\mathbf{k}_{\mathbf{x_{*}}\mathbf{X}}\left(\mathbf{K}_{\mathbf{X}\mathbf{X}}+\sigma_{o}^{2}\mathbf{I}\right)^{-1}\mathbf{1},$
(6)

where $\nabla\mathbf{k}_{\mathbf{x_{*}}\mathbf{X}}$ is the partial derivative
of the kernel matrix with respect to $\mathbf{x}$.

###  III-B OpenVDB Data Structure

The commonly used Octree data structure [9] usually divides the full scene
into spatial clusters in a multi-layer tree and treats each cluster
separately. However, as the scene grows, the Octree expands with more nodes,
and tree traversing gets slower as the access complexity $\mathcal{O}(\log n)$
is related to the number of nodes $n$. OpenVDB data structure (Volumetric,
Dynamic grid that shares several characteristics with B+trees) [14, 13] is
used in this work to enable efficient, large-scale, dense incremental mapping.
This hierarchical tree-like structure allows us to efficiently divide GP into
clusters with constant computational complexity to access them.

The B-tree-like three-dimensional VDB data structure comprises sparse
collections of voxel blocks in four layers: root nodes, leaf nodes, and
usually two levels of internal nodes. The default configuration is typical
$2^{3}\times 2^{3}\times 2^{3}$ voxels in each leaf node, $2^{4}\times
2^{4}\times 2^{4}$ of leaf nodes in a first-layer of internal node, and
$2^{5}\times 2^{5}\times 2^{5}$ of the first-layer of internal nodes in a
second-layer of internal node, Therefore, a second-layer of internal node
subsumes a three-dimensional block of voxels of size $4096\times 4096\times
4096$. Unlike the internal nodes and leaf nodes, the number of children of the
root node is not explicitly fixed. As default, each root node has $4$ second-
layer internal nodes. This hierarchical tree structure is fixed with four
height layers across the involved tree, maintaining a shallow and broad
representation compared to octrees. The constant height of the VDB tree allows
constant and fast traversing from the root node to the leaf node. Any random
access can operate at a consistent computational complexity in average
$\mathcal{O}(1)$. Furthermore, VDB substantially reduces contemporary CPU
memory consumption by hierarchically allocating the nodes even with extreme
grid resolution.

The memory and time efficiency properties of VDB [13] are highly suitable for
our online mapping frameworks. Note that each leaf node in VDB is a proper-
sized GP cluster, and we use the leaf iterator to traverse available voxels in
each leaf node and then use them to train each GP cluster separately.

Figure 2: Block Diagram of the proposed VDB-GPDF framework. We first model the
temporary Local Gaussian Process Signed Distance Field (L-GPDF) and surface
properties using $\mathcal{P}_{v\\{i\\}}$. A set of testing points along the
ray from the sensor origin to $\mathcal{P}_{v\\{i\\}}$ are generated to query
the distance and surface properties inferences of L-GPDF. Each point in
$\mathcal{P}_{t\\{i\\}}$ with its inferred $\hat{d}_{t}$ and $\hat{c}_{t}$ are
fused with the full map represented by a global VDB. Then we have all fused
voxels $\mathcal{P}_{\\{0,...,i\\}}$. The marching cube is applied on the
active voxels in $\mathcal{P}_{\\{0,...,i\\}}$ to update the zero crossing
points $\mathcal{P}_{\mathcal{S}\\{0,...,i\\}}$ and dense reconstruction. The
global Gaussian Process Signed Distance Field (G-GPDF) is modelled by
$\mathcal{P}_{\mathcal{S}\\{0,...,i\\}}$ in each leaf node in the global VDB
separately.

##  IV VDB-GPDF Framework

To incrementally build and maintain a persistent, efficient, large-scale,
adaptable, robust and dense distance field map of the environment with depth
sensors, we propose to couple GPDF [8] with local and global VDB data
structures. Fig. 2 shows the diagram of our proposed VDB-GPDF framework.

Depth sensor data is captured as a raw point cloud
${}^{{C}_{{i}}}\mathcal{P}_{\\{i\\}}$ in sensor frame ${C}_{{i}}$ at current
time $i$. The estimated pose is used to project the point cloud from current
sensor frame ${{C}_{{i}}}$ to world frame ${W}$ given the homogeneous
transformation matrix $\mathbf{T}_{{C}_{{i}}}$ yielding a point cloud
$\mathcal{P}_{\\{i\\}}$ in the world reference frame. The raw points
$\mathcal{P}_{\\{i\\}}$ are voxelised into the voxels denoted as
$\mathcal{P}_{v\\{i\\}}$ via a local hierarchical VDB. Thus,
$\mathcal{P}_{v\\{i\\}}$ is divided into local clusters maintained by the VDB
leaf nodes. For each leaf node, the centres of voxels are used as training
points to model GPs separately for distance and surface properties. All GPs
together form the temporary Local Gaussian Process Signed Distance Field
(L-GPDF) along with surface properties. In addition, $\mathcal{P}_{v\\{i\\}}$
is passed to the testing points generation block. A ray-cast operation from
the current sensor origin to each voxel in $\mathcal{P}_{v\\{i\\}}$ is
performed to generate a set of voxels. These voxels are then used as the
testing points $\mathcal{P}_{t\\{i\\}}$ to query the L-GPDF. Let us denote
each point in $\mathcal{P}_{t\\{i\\}}$ as $\mathbf{x_{*}}$. Note that the
distance sign of each $\mathbf{x_{*}}$ is computed given the sensor origin and
$\mathcal{P}_{v\\{i\\}}$. For each querying point $\mathbf{x_{*}}$, the
closest node in L-GPDF is found to compute the GP distance inference
${\hat{d}}_{t}(\mathbf{x_{*}})$, surface properties
$\hat{c}_{t}(\mathbf{x_{*}})$ and uncertainty.

The $\hat{d}_{t}$ and $\hat{c}_{t}$ estimates (mean and uncertainty) at each
location in $\mathcal{P}_{t\\{i\\}}$ are then fused with the existing map
maintained by a global VDB grid, which has all fused voxels
$\mathcal{P}_{\\{0,...,i\\}}$. The leaf nodes in the global VDB grid that
participated in the fusion procedure are marked as active. Marching cubes
algorithm [23] is then performed for the voxels in the active nodes to recover
the dense reconstruction. The zero crossing points
$\mathcal{P}_{\mathcal{S}\\{0,...,i\\}}$ in active nodes and previous active
nodes are used to generate the global Gaussian Process Signed Distance Field
(G-GPDF). Similar to L-GPDF, each leaf node is trained separately. The G-GPDF
is available to be queried for any given point in the set of all points the
space $\mathcal{P}_{o}$ of downstream applications that require accurate
distance $\hat{d}_{o}$ and gradients $\nabla\hat{d}_{o}$, _e.g._ , navigation,
path planning, localisation and manipulation.

###  IV-A Local Gaussian Process Signed Distance Field

We propose to efficiently voxelise the raw points $\mathcal{P}_{\\{i\\}}$ in
the world frame into a set of voxels via a local hierarchical VDB. The method
groups all raw points projecting into the same voxel. Therefore, a dense raw
point cloud $\mathcal{P}_{\\{i\\}}$ is represented by the centres of voxels,
denoting as $\mathcal{P}_{v\\{i\\}}$. We simply perform the further fusion and
integration once for each voxel. Taking advantage of efficient access to VDB,
the voxelisation procedure is fast and $\mathcal{P}_{v\\{i\\}}$ is
automatically divided into leaf nodes maintained by the VDB.

We iterate each leaf node of the local VDB for voxels, which are used as
training points to formulate each node as a GP separately. All GPs in local
VDB consist of the temporary Local Gaussian Process Signed Distance Field
(L-GPDF) along with surface properties. As explained in Section. III-A, given
locations of voxels, we compute the distance field inference and variance via
Eq. 4 and Eq. 5. In addition, given the voxels’ surface properties of each
leaf node, we propose to model the surface properties inference:

$\displaystyle\hat{c}_{t}(\mathbf{x}_{*})$
$\displaystyle={\mathbf{k}_{\mathbf{x}_{*}\mathbf{X}}}\left({\mathbf{K}_{\mathbf{X}\mathbf{X}}}+\sigma_{c}^{2}\mathbf{I}\right)^{-1}\mathbf{c}.$
(7)

Therefore, L-GPDF is ready for the testing points to query the distance,
surface properties and variance. Note that the temporary and local L-GPDF is
re-modelled at every frame $i$.

###  IV-B Testing Points Generation

Testing points generation method takes $\mathcal{P}_{v\\{i\\}}$ and sensor
origin as input. Here, we adopt a similar idea as the so-called free space
carving method [1, 3] to update the distance information for the voxels within
the current sensor field of view. Free-space carving has the ability to update
the dynamically changing objects in the scene. We propose three main
improvements to enhance efficiency while achieving comparable reconstruction
results to the conventional free-space carving used in most volumetric mapping
methods.

First, we propose a voxelisation method to ray-cast from the current sensor
origin to each voxel in $\mathcal{P}_{v\\{i\\}}$. This results in a faster
traverse than ray-casting for each raw point cloud $\mathcal{P}_{\\{i\\}}$ and
produces comparable zero crossing. Secondly, the naive ray-casting usually
iterates and updates all voxels intersected by the ray in order. This may lead
to updating the voxel without any surface measurement or the same voxel
multiple times due to having different ray intersections. In contrast, we only
query the voxel, which has previous surface measurements in the field of view.
In this way, the existence of the previous surface is updated. Therefore, the
dynamic objects are handled implicitly and efficiently. Note that testing all
points in the frustum is time-consuming and not necessary in our framework.
Despite we can get the fused ESDF this does not produce a generative model
that can be queried anywhere in the space. Therefore, we choose to infer only
the relevant testing points for fusion, making this process more efficient and
later generating the accurate ESDF using G-GPDF as explained in the next
section. Thirdly, we compute the surface normals given
$\mathcal{P}_{v\\{i\\}}$ and generate voxels along the normal direction up to
a certain distance even outside the frustum. When the sensor ray is close to
being parallel to the observed surface, the naive ray-casting cannot generate
enough testing voxels around the surface. Having extra voxels along the normal
direction yields a complete reconstruction.

We use $\mathcal{P}_{t\\{i\\}}$ to query the distance through the L-GPDF. For
each $\mathbf{x_{*}}$ in $\mathcal{P}_{t\\{i\\}}$, we find the closest node in
L-GPDF and use the node GP to compute the GP distance inference
$\hat{d}_{t}(\mathbf{x_{*}})$, surface property inference
$\hat{c}_{t}(\mathbf{x_{*}})$, and variances $\hat{v}_{t}(\mathbf{x_{*}})$ and
$\hat{w}_{t}(\mathbf{x_{*}})$. Thanks to the available Euclidean distance
inference of L-GPDF, we only query each voxel once to perform the fusion
later. This leads to a comparable result of the final reconstruction but more
efficiently than TSDF fusion. Note that we also compute the sign of distance
given the sensor origin and $\mathcal{P}_{v\\{i\\}}$.

###  IV-C Data Fusion

After querying the testing points from the L-GPDF, we have the signed distance
inference $\hat{d}_{t}(\mathbf{x_{*}})$, $\hat{c}_{t}(\mathbf{x_{*}})$ and
variance $\hat{v}_{t}(\mathbf{x_{*}})$ $\hat{w}_{t}(\mathbf{x_{*}})$ for each
$\mathbf{x_{*}}$ in $\mathcal{P}_{t\\{i\\}}$. Unlike the commonly used TSDF
fusion that uses constant or drop-off weights to fuse the projective (or non-
projective) distance, we directly query our probabilistic L-GPDF to fuse the
distance and surface properties through its mean and variance in the global
VDB. Note that the fusion is mostly done around the surface, as our variance
is only accurate near the surface, as discussed in Section III.

The global VDB efficiently accesses the previous distance mean and weight at
$\mathbf{x_{*}}\in\mathcal{P}_{t\\{i\\}}$, which are denoted as
$\boldsymbol{D}_{i-1}(\mathbf{x_{*}})$ and
$\boldsymbol{V}_{i-1}(\mathbf{x_{*}})$ respectively. Then
$\boldsymbol{D}_{i}(\mathbf{x_{*}})$ and $\boldsymbol{V}_{i}(\mathbf{x_{*}})$
is estimated given the new queried $\hat{d}_{t}(\mathbf{x_{*}})$
$\hat{v}_{t}(\mathbf{x_{*}})$ of L-GPDF. We then perform the distance fusion
following the standard weighted sum as in [24],

$\displaystyle\boldsymbol{D}_{i}(\mathbf{x_{*}})$
$\displaystyle=\frac{\boldsymbol{V}_{i-1}(\mathbf{x_{*}})\cdot\boldsymbol{D}_{i-1}(\mathbf{x_{*}})+(1-\hat{v}_{t}(\mathbf{x_{*}}))\cdot\hat{d}_{t}(\mathbf{x_{*}})}{\boldsymbol{V}_{i-1}(\mathbf{x_{*}})+(1-\hat{v}_{t}(\mathbf{x_{*}}))}$
(8) $\displaystyle\boldsymbol{V}_{i}(\mathbf{x_{*}})$
$\displaystyle=\boldsymbol{V}_{i-1}(\mathbf{x_{*}})+(1-\hat{v}_{t}(\mathbf{x_{*}})).$

Similarly, the surface properties are fused using

$\displaystyle\boldsymbol{C}_{i}(\mathbf{x_{*}})$
$\displaystyle=\frac{\boldsymbol{W}_{i-1}(\mathbf{x_{*}})\cdot\boldsymbol{C}_{i-1}(\mathbf{x_{*}})+(1-\hat{w}_{t}(\mathbf{x_{*}}))\cdot\hat{c}_{t}(\mathbf{x_{*}})}{\boldsymbol{W}_{i-1}(\mathbf{x_{*}})+(1-\hat{w}_{t}(\mathbf{x_{*}}))}$
(9) $\displaystyle\boldsymbol{W}_{i}(\mathbf{x_{*}})$
$\displaystyle=\boldsymbol{W}_{i-1}(\mathbf{x_{*}})+(1-\hat{w}_{t}(\mathbf{x_{*}})).$

###  IV-D Global Gaussian Process Signed Distance Field

After the fusion process, the leaf nodes in the global VDB grid are marked as
active nodes. We perform the marching cubes for the voxels of active nodes and
for computing the dense reconstruction with surface properties. The zero
crossing points in current active nodes $\mathcal{P}_{\mathcal{S}\\{i\\}}$
then merge into $\mathcal{P}_{\mathcal{S}\\{0,...,i\\}}$. Zero crossing points
in each node are used to train separate GPs. The combination of all GPs forms
the global Gaussian Process Signed Distance Field (G-GPDF).

Note that when querying the G-GPDF, we only search for a certain number of
closest nodes through a kd-tree for each querying point and only perform the
corresponding GP training and inference for these nodes. In this way, we avoid
training unnecessary GP nodes which no points are querying them. This will
avoid redundancy in computing the GP of the entire field. In addition, we use
the smooth minimum function for the distance field to ensure accurate global
consistency.

When partitioning the full map into leaf nodes in the VDB data structure,
modelling the clustering GP of each node suffers from discontinuity around the
boundaries. A common solution is to have overlapping parts with neighbouring
nodes. However, this method extends the volume of each node therefore the GP
training becomes more complex in terms of computations. Instead, we address
the problem by using the smooth minimum function to estimate the continuous
distance field. We search for the number of $Q$ clustered GP nodes in the map
and recover the distance as,

${\hat{d}}(\mathbf{x}_{*})=\frac{\sum_{q=1}^{Q}{\hat{d}}_{q}(\mathbf{x}_{*})\exp\left(\lambda{\hat{d}}_{q}(\mathbf{x}_{*})\right)}{\sum_{q=1}^{Q}\exp\left(\lambda{\hat{d}}_{q}(\mathbf{x}_{*})\right)},$
(10)

where $\lambda$ is a large positive value to control the evenness of the
smooth minimum. We use $100$ for $\lambda$ in this paper.
${\hat{d}}_{q}(\mathbf{x}_{*})$ is the distance inference at querying location
$\mathbf{x}_{*}$ given the located GP node with the index of $q$. For the
gradient, we directly average the inferred gradients from the $Q$ clustered GP
nodes.

(a)

(b)

(c)

(d)

(e)

(f)

Figure 3: Quantitative comparisons of a) and b) the efficiency performance, c)
and d) reconstruction accuracy in Chamfer distance, e) and f) the distance
field accuracy on the cow and lady RGB-D dataset and the newer college LiDAR
dataset.

##  V Evaluation

To evaluate the proposed VDB-GPDF framework, we quantitatively compare the
mapping performance for a) efficiency, b) reconstruction accuracy and c)
distance accuracy, and qualitatively demonstrate the mapping performance for a
room size with RGB-D cameras and a LiDAR large-scale dataset. Our framework is
implemented in C++ based on ROS1. All experiments were run on 12th Gen Intel®
Core™ i5-1245U with 12 cores.

The first dataset is the Cow and
Lady111https://projects.asl.ethz.ch/datasets/doku.php?id=iros2017, which
includes fibreglass models of a large cow and a lady standing side by side in
a room. The dataset consists of RGB-D point clouds collected using a Kinect 1
camera, with sensor trajectory captured using a Vicon motion system. We use
frames 220-820 of the dataset, which covers major information of the scene.
For the second dataset, we choose the Newer College dataset [25] collected by
a handheld sensor platform with a 128-beam ouster LiDAR. The estimated poses
are calculated via the alignment of each frame to the ground truth map. The
size of the main quad is roughly $56\times 40\times 20$ m, with approximately
2000 frames. Both Cow and Lady and Newer College have the ground truth map
that allows us to perform a proper quantitative evaluation for both
reconstruction and distance field accuracy. For a fair comparison, we process
every frame from both datasets in each framework. Our strategy for length
scale selection depends on the number of voxels in each leaf node, which in
our case is fixed to $2^{3}\times 2^{3}\times 2^{3}$ voxels. A length scale
that correlates two to four voxels is a good trade-off between smoothing and
interpolation for our kernel function.

###  V-A Fusion Performance

First, we want to evaluate the proposed fusion. We compare the proposed ESDF
fusion with TSDF fusion in VDBFusion. For the sake of fairness, we apply the
same voxelisation method in Sec. IV-B for VDBFusion as in our proposed method.
A qualitative comparison of the TSDF fusion of VDBFusion and the proposed ESDF
fusion with full testing voxels in the frustum is demonstrated in Fig.
LABEL:tsdf_fusion and LABEL:esdf_fusion. The white boxes are the measurements,
and the coloured flat squares are the inferred distance values of testing
points for fusion in the field of view. The results show that our ESDF fusion
produces accurate and natural inferred distance of each point in the frustum.
After performing the fusion for all frames, we compare the reconstructed mesh
for TSDF fusion and our fusion. Our reconstruction in LABEL:esdf_fusion_mesh
shows less noise in the wall, more complete on the mattress and cow legs than
LABEL:tsdf_fusion_mesh, lower reconstruction error in Chamfer distance.

(a)

(b)

(c)

(d)

Figure 4: Qualitative and quantitative comparison of a) the TSDF fusion from
VDBFusion and b) our proposed ESDF fusion with full testing points in the
frustum. b) shows that our approach reasons directly in the Euclidean space.
After the fusion, our reconstruction in d) shows less noise in the wall and is
more complete on the mattress and cow legs than c).

###  V-B Efficiency

We first evaluate the VDB-GPDF efficiency against the state-of-the-art mapping
frameworks VDB-Fusion and VDB-EDT, both developed based on OpenVDB data
structure. Fig. LABEL:cow:time and LABEL:newer:time show the average
computational time per frame for the room size RGB-D Cow and Lady dataset and
the outdoor LiDAR Newer College dataset, respectively. Note that the Newer
College quad has pedestrians walking by as dynamic objects. Both frameworks
have free-space carving methods enabled to update the moving objects in the
scene. For the sake of simplicity, we compute the average time for all the
integration processes, including fusion and ESDF. As for our framework, we
include L-GPDF, incremental fusion and G-GPDF. Both figures show that our VDB-
GPDF is more efficient than other frameworks with varying resolutions due to
our voxelisation method and fewer testing points in the frustum area.
VDBFusion consumes more time in the RGB-D dataset than others due to enabling
the free-space carving process. For the LiDAR data, VDB-EDT requires more time
to update the EDF every frame.

###  V-C Reconstruction Accuracy

(a)

(b)

(c)

Figure 5: Qualitative reconstruction comparison of a) VDBFusion and b) our
proposed method shows that VDB-GPDF produces a more complete and natural
reconstruction, especially when the sensor is in parallel with the observed
surface. Our mesh covers on the top of the mattress, and the colour is
properly fused for visualisation as the scene photo colour in c).

VDBFusion and VDB-GPDF both produce similar meshes of the scene shown in Fig.
LABEL:cow_mesh_vdbfusion and Fig. LABEL:cow_mesh_ours, respectively. VDB-GPDF
produces a more complete and natural reconstruction, especially when the
sensor is parallel to the observed surface. It can be seen that there is a gap
at the top of the mattresses on the left-hand side in the VDBFusion result,
whereas VDB-GPDF does not have the same gap. In addition, our colour is nicely
fused with a sample of the RGB camera data in c) for qualitative comparison.
Note that, for example, around the wall and floor, our method shows the colour
with high fidelity, while VDBFusion colour is more approximate.

We use the Chamfer distance metrics [26] against VDB-Fusion and VDB-EDT to
compare the maps on both datasets quantitatively. As demonstrated in Fig.
LABEL:cow:chamfer and LABEL:newer:chamfer, our map quality outperforms VDB-EDT
in different resolutions. Due to our voxelisation method being applied, we
have comparable results to VDBFusion in reconstruction accuracy. To show the
performance in a large-scale dataset, we use the main quad of the Newer
College, which is roughly $56\times 40\times 20$ m, with approximately 2000
frames in the dataset. In Fig. 1, we qualitatively show our mesh coloured by
fused LiDAR intensity and the distance field output. Fig. LABEL:teaser1 shows
the incrementally built dense reconstruction. We zoomed in Fig. LABEL:teaser2
to show the textures on the ceiling inside the corridor and Fig. LABEL:teaser3
the stairs and windows at the corner of the quad.

(a)

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

(i)

(j)

Figure 6: Comparison of ground truth and estimated EDF with a 2D horizontal
slice 0.9 m above the ground for the Cow and Lady dataset. The resolution is
0.05m. e) is accurate and closely resembles the ground truth in a). b). c) and
d) do not produce enough smoothness in the field and only offer values in the
observed area. Our VDB-GPDF with G-GPDF in j) produces lower and smoother
values of distance errors than VDB-EDT in g) and Voxblox in h) and our method
without the G-GPDF in i).

(a)

(a)

(b)

(c)

(d)

(e)

(f)

(g)

(h)

Figure 7: Comparison of ground truth and estimated EDF with a 2D horizontal
slice 0.9 m above the ground for the newer college. d) is accurate and closely
resembles the ground truth in a). b) fails to update in the areas where moving
objects are present. c) does not produce enough smoothness in the field.
Similarly, Our VDB-GPDF in h) produces lower and smoother values of distance
errors than VDB-EDT in f) and Voxblox in g) with the LiDAR dataset.

###  V-D Distance Accuracy

We evaluate the proposed GP distance field with VDB structure against Voxblox
and VDB-EDT since the VDBFusion framework has no EDF. We use the ground truth
point cloud to compute the distance field ground truth numerically. We compute
the distance field on a regular 3D grid of 5cm resolution for the RGB-D case
and 10 cm for the LiDAR case. The RMSE error given the ground truth distance
field is shown in Fig. LABEL:cow:distance and LABEL:newer:distance. Our
distance field has the most accurate results. Voxblox produces the large
distance RMSE for the RGB-D dataset, and VDB-EDT has a larger error on the
Newer College dataset than others due to failure to clear the moving objects
successfully.

To further examine the behaviour of VDB-GPDF, we compare 2D slices of the
ground-truth and estimated distance fields in Fig. 6. We choose a horizontal
slice $0.9$m above the ground, roughly cutting the scene objects in the
middle. In addition, we evaluate the EDF with and without the G-GPDF enabled
to show that G-GPDF produces more accurate distance inference than fusing the
ESDF directly. Moreover, G-GPDF is a generative model that can generate
distance and gradient inference even in places with no observations. As shown
in Fig. LABEL:distance_c:edt, LABEL:distance_c:vox and Fig.
LABEL:distance_c:ours_direct_mode, VDB-EDT and Voxblox and VDB-GPDF without
G-GPDF only compute the EDF within the sensor’s field of view, whereas VDB-
GPDF with G-GPDF naturally predicts the EDF value at all points as can be seen
in Fig. LABEL:distance_c:ours. For a quantitative evaluation, we illustrate
the distance error with a colour map in Fig. LABEL:distance_c:error_edt,
LABEL:distance_c:error_vox, LABEL:distance_c:error_ours_direct_mode and
LABEL:distance_c:error_ours. Our VDB-GPDF produces lower and smoother values
of distance errors.

Similarly, for the LiDAR dataset, we compare 2D slices of the ground-truth and
estimated distance fields in Fig. 7. As shown in Fig. LABEL:distance_n:edt,
LABEL:distance_n:vox and LABEL:distance_n:ours, our VDB-GPDF computes the
similar EDF as the ground truth in Fig. LABEL:distance_n:ground_truth. For a
quantitative evaluation, we plot the same distance absolute error in Fig.
LABEL:distance_c:error_edt, LABEL:distance_c:error_vox and
LABEL:distance_c:error_ours. Our VDB-GPDF generates lower and more consistent
distance error values.

##  VI Conclusion

We propose VDB-GPDF, a framework that couples VDB structure and GPDF for
online incremental mapping. It has the scalability to build a large-scale
scene representation and is compatible with different types of depth sensors.
It offers a comparable reconstruction of the scene environment and accurate
Euclidean distance against state-of-the-art approaches. VDB-GPDF can
implicitly adapt to dynamic changes and probabilistically handle noisy
measurements. VDB-GPDF provides flexible multiple outputs such as Euclidean
distance and gradient fields, surface properties including colour and
intensity, and a dense mesh with informative textures. Future work considers
unlocking the ability to predict motion using the efficiently built changing
distance field.

UTS

    University of Technology Sydney
RI

    Robotics Institute
FEIT

    Faculty of Engineering and Information Technology
CAS

    Centre for Autonomous Systems
WiEIT

    Women in Engineering and IT
1D

    One-Dimensional
2D

    Two-Dimensional
2.5D

    Two-and-a-Half-Dimensional
3D

    Three-Dimensional
GP

    Gaussian Process
GPIS

    Gaussian Process Implicit Surfaces
MAP

    Maximum A Posteriori
MLE

    Maximum Likelihood Estimation
ICP

    Iterative Closest Point
MVE

    Multi-View Environment
OG

    Occupancy Grid
CHOMP

    Covariant Hamiltonian Optimization for Motion Planning
FIESTA

    Fast Incremental Euclidean DiSTAnce Fields
GPUs

    Graphics Processing Units
KD

    K-Dimensional
GPDF

    Gaussian Process Distance Field
Log-GPIS

    Log-Gaussian Process Implicit Surfaces
LiDAR

    Light Detection And Ranging Sensor
SLAM

    Simultaneous Localisation and Mapping
TDF

    Truncated Distance Field
EDF

    Euclidean Distance Field
SDF

    Signed Distance Field
TSDF

    Truncated Signed Distance Field
ESDF

    Euclidean Signed Distance Field
GPIS-SDF

    GPIS with signed distance function
RGB

    Red-Green-Blue
RGB-D

    Red-Green-Blue-Depth
RMSE

    Root Mean Squared Error
CI

    Conditional Independent
FP

    Forward Propagation
BP

    Backward Propagation
D-SKI

    Structured Kernel Interpolation framework with Derivatives
SKI

    Structured Kernel Interpolation method
D-SKI-CI-Fusion

    Structured Kernel Interpolation with Derivatives and Conditional Independent Fusion
MVMs

    Matrix-Vector Multiplications method
PDE

    Partial Differential Equation
Log-GPIS-MOP

    Log-Gaussian Process Implicit Surface for Mapping, Odometry and Planning
RANSAC

    Random Sample Consensus
Dynamic-GPDF

    Dynamic Gaussian Process Distance Fields

## References

  * [1] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3D Euclidean Signed Distance Fields for on-board MAV planning,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, Sep. 2017.
  * [2] L. Han, F. Gao, B. Zhou, and S. Shen, “FIESTA: Fast Incremental Euclidean Distance Fields for Online Motion Planning of Aerial Robots,” 2019.
  * [3] I. Vizzo, T. Guadagnino, J. Behley, and C. Stachniss, “Vdbfusion: Flexible and efficient tsdf integration of range sensor data,” _Sensors_ , vol. 22, no. 3, p. 1296, 2022.
  * [4] D. Zhu, C. Wang, W. Wang, R. Garg, S. Scherer, and M. Q.-H. Meng, “VDB-EDT: An Efficient Euclidean Distance Transform Algorithm Based on VDB Data Structure,” May 2021. [Online]. Available: https://arxiv.org/abs/2105.04419v1
  * [5] Y. Bai, Z. Miao, X. Wang, Y. Liu, H. Wang, and Y. Wang, “Vdbblox: Accurate and efficient distance fields for path planning and mesh reconstruction,” in _2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2023, pp. 7187–7194.
  * [6] L. Wu, K. M. B. Lee, L. Liu, and T. Vidal-Calleja, “Faithful Euclidean Distance Field from Log-Gaussian Process Implicit Surfaces,” Jan. 2021, arXiv:2010.11487 [cs]. [Online]. Available: http://arxiv.org/abs/2010.11487
  * [7] L. Wu, K. M. B. Lee, C. Le Gentil, and T. Vidal-Calleja, “Log-GPIS-MOP: A Unified Representation for Mapping, Odometry, and Planning,” _IEEE Transactions on Robotics_ , pp. 1–17, 2023. [Online]. Available: https://ieeexplore.ieee.org/document/10202666/
  * [8] C. L. Gentil, O.-L. Ouabi, L. Wu, C. Pradalier, and T. Vidal-Calleja, “Accurate Gaussian Process-based Distance Fields with applications to Echolocation and Mapping,” Sep. 2023, arXiv:2302.13005 [cs] version: 2. [Online]. Available: http://arxiv.org/abs/2302.13005
  * [9] B. Lee, C. Zhang, Z. Huang, and D. D. Lee, “Online continuous mapping using gaussian process implicit surfaces,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019.
  * [10] U. Ali, L. Wu, A. Mueller, F. Sukkar, T. Kaupp, and T. Vidal-Calleja, “Interactive distance field mapping and planning to enable human-robot collaboration,” _arXiv preprint arXiv:2403.09988_ , 2024.
  * [11] B. Lau, C. Sprunk, and W. Burgard, “Improved updating of euclidean distance maps and voronoi diagrams,” in _2010 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2010, pp. 281–286.
  * [12] Y. Pan, Y. Kompis, L. Bartolomei, R. Mascaro, C. Stachniss, and M. Chli, “Voxfield: Non-Projective Signed Distance Fields for Online Planning and 3D Reconstruction,” in _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. Kyoto, Japan: IEEE, Oct. 2022, pp. 5331–5338.
  * [13] K. Museth, J. Lait, J. Johanson, J. Budsberg, R. Henderson, M. Alden, P. Cucka, D. Hill, and A. Pearce, “Openvdb: an open-source data structure and toolkit for high-resolution volumes,” in _Acm siggraph 2013 courses_ , 2013, pp. 1–1.
  * [14] K. Museth, “Vdb: High-resolution sparse volumes with dynamic topology,” _ACM transactions on graphics (TOG)_ , vol. 32, no. 3, pp. 1–22, 2013.
  * [15] K. Goel and W. Tabib, “Distance and collision probability estimation from gaussian surface models,” _arXiv preprint arXiv:2402.00186_ , 2024.
  * [16] J. J. Park, P. Florence, J. Straub, R. Newcombe, and S. Lovegrove, “Deepsdf: Learning continuous signed distance functions for shape representation,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 165–174.
  * [17] A. Gropp, L. Yariv, N. Haim, M. Atzmon, and Y. Lipman, “Implicit geometric regularization for learning shapes,” _arXiv preprint arXiv:2002.10099_ , 2020.
  * [18] M. Pantic, C. Cadena, R. Siegwart, and L. Ott, “Sampling-free obstacle gradients and reactive planning in neural radiance fields (nerf),” _arXiv preprint arXiv:2205.01389_ , 2022.
  * [19] J. Ortiz, A. Clegg, J. Dong, E. Sucar, D. Novotny, M. Zollhoefer, and M. Mukadam, “isdf: Real-time neural signed distance fields for robot perception,” _arXiv preprint arXiv:2204.02296_ , 2022.
  * [20] J. Huang, S.-S. Huang, H. Song, and S.-M. Hu, “Di-fusion: Online implicit 3d reconstruction with deep priors,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2021, pp. 8932–8941.
  * [21] V. Vasilopoulos, S. Garg, J. Huh, B. Lee, and V. Isler, “Hio-sdf: Hierarchical incremental online signed distance fields,” _arXiv preprint arXiv:2310.09463_ , 2023.
  * [22] C. E. Rasmussen and C. K. Williams, _Gaussian Processes for Machine Learning_. Cambridge, Mass.: MIT Press, 2006.
  * [23] W. E. Lorensen and H. E. Cline, “Marching cubes: A high resolution 3d surface construction algorithm,” in _Proceedings of the 14th Annual Conference on Computer Graphics and Interactive Techniques_ , ser. SIGGRAPH ’87. Association for Computing Machinery, 1987.
  * [24] B. Curless and M. Levoy, “A volumetric method for building complex models from range images,” in _Proceedings of the 23rd Annual Conference on Computer Graphics and Interactive Techniques_ , ser. SIGGRAPH ’96. Association for Computing Machinery, 1996, p. 303–312.
  * [25] M. Ramezani, Y. Wang, M. Camurri, D. Wisth, M. Mattamala, and M. Fallon, “The newer college dataset: Handheld lidar, inertial and vision with ground truth,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020, pp. 4353–4360.
  * [26] L. Mescheder, M. Oechsle, M. Niemeyer, S. Nowozin, and A. Geiger, “Occupancy networks: Learning 3d reconstruction in function space,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 4460–4470.

  *[TSDF]: Truncated Signed Distance Field
  *[ESDF]: Euclidean Signed Distance Field
  *[GP]: Gaussian Process
  *[EDF]: Euclidean Distance Field


# Hierarchical Learned Risk-Aware Planning Framework for Human Driving
Modeling

Nathan Ludlow1 Yiwei Lyu2 and John Dolan2 1Robotics and Dynamics Laboratory,
Brigham Young University, Provo, Utah 84602, USA<EMAIL_ADDRESS>Center for
Autonomous Vehicle Research, Carnegie Mellon University, Pittsburgh 15289, USA
[yiweilyu<EMAIL_ADDRESS>
###### Abstract

This paper presents a novel approach to modeling human driving behavior,
designed for use in evaluating autonomous vehicle control systems in a
simulation environments. Our methodology leverages a hierarchical forward-
looking, risk-aware estimation framework with learned parameters to generate
human-like driving trajectories, accommodating multiple driver levels
determined by model parameters. This approach is grounded in multimodal
trajectory prediction, using a deep neural network with LSTM-based social
pooling to predict the trajectories of surrounding vehicles. These
trajectories are used to compute forward-looking risk assessments along the
ego vehicle’s path, guiding its navigation. Our method aims to replicate human
driving behaviors by learning parameters that emulate human decision-making
during driving. We ensure that our model exhibits robust generalization
capabilities by conducting simulations, employing real-world driving data to
validate the accuracy of our approach in modeling human behavior. The results
reveal that our model effectively captures human behavior, showcasing its
versatility in modeling human drivers in diverse highway scenarios.

###### Index Terms:

Human modeling; Risk-aware; Social Pooling; Trajectory Prediction; Autonomous
Vehicles; Simulation; Robotics

©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

As the presence of autonomous vehicles on our roads becomes more common,
rigorous testing of safety algorithms is essential for the protection of all
road users. Real-world testing poses numerous challenges, including cost,
time, and safety concerns, particularly in scenarios with potential near-
collision events. While real-world validation of systems remains essential,
simulations offer a compelling alternative, providing safety, efficiency,
cost-effectiveness, and the ability to test and optimize systems under various
challenging scenarios without physical risk.

However, a significant challenge in simulation testing stems from the
unpredictable nature of human drivers, who often cause near-collision
situations involving autonomous vehicles. To ensure accurate and effective
simulation testing of autonomous vehicle systems, it’s crucial to integrate a
precise model of human driving behavior. This ensures that algorithms can
adeptly handle real-world unpredictability, ultimately enhancing road safety.

Despite numerous proposed approaches for modeling human behavior, no widely
adopted comprehensive model of human driving behavior exists, largely due to
limitations in existing models. Human drivers typically plan their actions
many seconds in advance, considering numerous driving factors and potential
actions of surrounding vehicles, such as lane selection, overtaking decisions,
and yielding to oncoming traffic [1]. To accurately replicate human-like
driving behavior for advanced maneuvers, we propose the need to plan the ego
vehicle’s trajectory several seconds into the future. This planning must
account for the multimodal nature of the behavior of surrounding vehicles
while optimizing for a safe trajectory, considering all potential surrounding
vehicle actions. Many existing human-driving models have limited planning
horizons and single-mode surrounding vehicle predictions, which hinder their
ability to execute higher-level human behaviors and navigate safely around
multiple vehicles. Leveraging an intuitive understanding of how human drivers
plan their vehicle trajectories, our proposed model offers improved
capabilities in modeling human driving behavior.

To achieve this, we propose a novel hierarchical learned risk-aware planning
framework. Our framework uses a recurrent neural network-based trajectory
prediction with social pooling method to predict the actions of all
surrounding vehicles. Then to capture the interactive risk the ego vehicle
faces we propose a matrix to represent the aggregated risk at each point along
the highway from a multimodal distribution of possible trajectories from all
surrounding vehicles. We use Next Generation Simulation (NGSIM) [2] data to
learn parameters that determine human driving and feed those into the risk-
aware planning cost function to more accurately represent human driving based
on these learned parameters. Since our proposed model is able to plan a long-
term trajectory in real-time, this framework is effective at capturing high-
level human behaviors, while being sufficiently computationally to be used in
simulations with a high number of agents. Additionally, the model includes a
parameter for maximum allowable risk, allowing the model to be adjusted to
represent different styles of driving, increasing the accuracy of the model in
various situations.

Our main contributions are: (1) A novel forward-looking risk metric based on
the hierarchical combination of social pooling and history-based trajectory
prediction and risk evaluation along predicted trajectories that evaluates
risk based not only on current vehicle states, but also predicted future
vehicle states that are estimated based on vehicle history and vehicle
interaction effects; (2) An accurate human model adjustable with parameters
found from data-driven approaches that can represent multiple styles of
driving behaviors while being computationally efficient, allowing it to be
used in real-time risk assessment and planning of autonomous vehicles. (3) An
assessment of the hierarchical learned risk-aware planning framework’s
performance using real-world data, coupled with simulation-based examinations
of its capacity to replicate human driving behavior, accompanied by an
analysis of human driver conduct within specific contextual scenarios.

##  II Related Work

Rule-based approaches attempt to model human driving behavior by intuiting how
a human driver will maneuver their vehicle and defining a comprehensive set of
rules to represent that behavior. These approaches often generalize well and
can easily generate new behaviors not previously observed in datasets. These
models also have the benefit of generating easily interpretable trajectories.
There are many rule-based approaches for modeling human behaviors such as the
Intelligent Driver Model (IDM) [3], Game-theoretic planning [4, 5], and risk-
aware planning [6, 7]. While the IDM works well in its context, it can only
replicate lane following without lane changing, making it unable to replicate
complex human behavior. Other approaches can be used to build comprehensive
driving models, but fail to driving behavior that is human-like [8].

Learning-based approaches to modeling human driving behavior generally use
Inverse Reinforcement Learning [9, 10, 11] or Imitation Learning [11, 12, 13]
to learn cost functions for human drivers or to replicate expert driving
maneuvers, respectively. Both of these methods perform reasonably at
replicating human driving behavior when tested in situations already observed
in data; however, these methods fail to generalize well to situations and
contexts not seen in training data [14]. Additionally, learning-based methods
require large amounts of data for training, which is not always available
[15]. Another problem of these learning-based models is that due to the
complexity of driving they fail to learn high-level behaviors of vehicles, and
only effectively to capture the specific maneuvers of individual vehicles
[14].

Human driving behavior depends on a number of factors; however, it has been
shown that human drivers operate their vehicles in large part based on
perceived risk [7, 16, 17, 18]. These studies however, do not attempt to use
risk modeling to build a comprehensive model of human driving and instead only
show that certain high level behaviors can be predicted from the risk the ego
vehicle experiences. In our work, we assume that drivers operate their vehicle
primarily based on risk [19, 20, 21], and perceived optimality to the driver
of a path, and we propose that we can develop an effective driver model with
this method. The NGSIM dataset provides valuable information on human driving
behavior with proper filtering of data [22, 23, 24]. By using these data, we
observe human driver tendencies and behaviors to learn and validate models on
real-world data. Our novel approach to modeling human driving behavior has
distinct advantages over existing methods in multiple key areas. By predicting
the trajectories of surrounding vehicles multiple seconds into the future, we
enable the risk-aware planner to plan for many maneuvers that may be less
optimal in the short term but yield long-term rewards, similar to how human
drivers often operate their vehicles. Additionally, by learning parameters for
the risk-aware planning we can gain many of the benefits of both rule-based
and learned models by having a well defined set of driving rules that are
generalized to perform well in all road contexts and situations even when the
model has not been trained on the specific scenario, while still accurately
representing human driving behavior. Lastly, by having adjustable parameters
for maximum allowable risk our framework can represent different styles of
driving based on the aggressiveness specified for the driver model.

##  III Method

###  III-A Trajectory Prediction

Convolutional Social Pooling for trajectory prediction is the means of
estimating a set of probable trajectories for a vehicle [25]. The trajectory
prediction is performed by a set of Long Short-Term Memory (LSTM) networks
that are used to encode the histories of surrounding vehicles into a social
occupancy grid. These histories are then passed through multiple convolutional
and pooling layers before being decoded by another LSTM network into predicted
trajectories. The predictor outputs six individual trajectories, representing
three lateral maneuvers: left lane change, keep lane, and right lane change;
and two longitudinal maneuvers: maintaining velocity or decelerating. Each of
these trajectories is assigned an associated probability based on its
likelihood of occurrence. The Convolutional Social Pooling approach from [25]
is employed for its ability to model human driving behavior, its computational
efficiency for real-time predictions, and its multimodal output. While
Convolutional Social Pooling is well-suited for this purpose, other
sufficiently accurate multimodal trajectory prediction methods could be
adapted to fit this framework.

###  III-B Risk Measurement

Our approach considers all other agents to be dynamic obstacles with respect
to the ego agent. We consider a bounded environment $Q\subset\mathbb{R}^{2}$
with points $q\in Q$. The positions of $N_{agents}$ agents in the environment
are denoted as $p_{j}$ with $j=\\{1,...,N_{agents}\\}$, and velocities of the
agents are denoted with $u_{j}$. The position of the ego agent is denoted as
$p_{e}$ and the velocity as $u_{e}$. Each agent is modeled as a kinematic
bicycle model with acceleration $a_{i}$ and steering $\theta_{i}$ the control
input for agent $j$. To account for the uncertain multimodal nature of human
driving we use the trajectory prediction module introduced above [25] to
generate a series of potential trajectories for each actor in the scene at
future time steps $t_{i}$ spaced evenly $t_{s}$ apart where
$i=\\{1,...,N_{time}\\}$ and $N_{time}$ is the number of predicted vehicle
positions. The prediction module can account for interaction effects of other
vehicles as well as vehicle histories to provide a more accurate model of
potential actions taken by surrounding vehicles. Additionally the trajectory
prediction module accounts for multimodality in the potential human actions by
generating $N_{traj}$ possible trajectories with the associated probability
$P_{ik}$ with $k=\\{1,...,N_{traj}\\}$, indicating the normalized likelihood
the vehicle will follow the associated trajectory, bounded within the range of
$[0,1]$. Each trajectory is composed of a series of points $p_{ijk}$ spaced
$\Delta t$ apart in time, indicating the expected position of the vehicle.
This multimodal risk prediction allows us to plan the ego vehicle trajectory
based on multiple probable futures for each vehicle similar to how human
drivers operate their vehicles.

$\displaystyle L_{\bar{P},i}=\\{q\ |\
\mathcal{H}_{c,i}(q,p,\dot{p})\leq\mathcal{H}_{P}\\}$ (1)

For each future time step $t_{i}$ of the predicted future trajectories of each
trajectory we represent the risk at each point $q$ by defining a risk cost as
$\mathcal{H}_{c,i}$. As in [7], the risk cost represents the risk posed to any
point along the road relative to the positions and velocities of all
surrounding agents. We define a planning threshold as $\mathcal{H}_{P}$ as the
maximum risk the ego vehicle navigate through. This leads to the definition of
a risk level set $L_{\bar{P,i}}$ represented in Equation 1 at a future time
step $t_{i}$ which is the set of all points less that the chosen planning
threshold $\mathcal{H}_{P}$ that the ego vehicle uses when planning.

Figure 1: A visualization of the normalized risk computed using the higher-
order rectangular Gaussian function for a single vehicle at one time step.
Lower risk areas are visualized in blue, and higher risk areas in yellow.

$\displaystyle\mathcal{H}_{c}(q,p_{i},\dot{p_{i}})=$ (2)
$\displaystyle\sum^{n}_{i=1}{\dfrac{exp\left(-\left(\dfrac{(q_{x}-p_{x,i})^{2}}{\sigma_{x,i}^{2}}\right)^{\beta}-\left(\dfrac{(q_{x}-p_{y,i})^{2}}{\sigma_{y,i}^{2}}\right)^{\beta}\right)}{1+exp(\alpha\dot{p}_{i}^{T}(q-p_{i}))}}$

To compute $\mathcal{H}_{c,i}$ we use rectangular higher-order Gaussians from
the work in [7] represented in Equation 2 with
$\sigma_{i}=\dfrac{w_{i}}{2}+|\dot{p}_{i}|$ for agent $i$ with $w_{i}$
representing the size of the agent modeled as a rectangle. We choose to use
the rectangular Gaussian as opposed to the elliptical variant, as it weights
the risk equally along all points of the body of the agent. This approach
allows our method to accurately compute the risk surrounding vehicles of any
length or width along the road. Additionally the higher-order Gaussian method
of computing risk takes into account both the position and velocity of each
agent, meaning that a vehicle with a higher velocity will cause higher risk at
a point than an equivalent vehicle with a lower velocity. This intuitively
means that as other agent vehicles travel faster the ego vehicle will plan to
stay further from them, similar to how human drivers operate their vehicles.

We modify the rectangular cost function to include the probability $P_{jk}$ of
the trajectory, since trajectories that are less probable to be taken by the
surrounding vehicles cause less risk. As used in [7], we use $\alpha=0.8$ and
$\beta=1.5$, since this provided the best fit to the data based on the
behavior from cars in the NGSIM data. By modifying the cost function to take
into account the probability of the trajectory being computed we allow the ego
vehicle to prioritize avoiding paths that would collide with highly probable
trajectories of surrounding vehicles, while also planning to avoid paths that
could collide with low likelihood trajectories of surrounding vehicles when
there is sufficient space on the road available.

$\displaystyle\mathcal{H}_{c,i}(q,p_{i},\dot{p_{i}})=\sum^{N_{agents}}_{j=1}\sum^{N_{traj}}_{k=1}\cdot
P_{ij}\cdot$ (3)
$\displaystyle\dfrac{exp\left(-\left(\dfrac{(q_{x}-p_{x,ijk})^{2}}{\sigma_{x,ijk}^{2}}\right)^{\beta}-\left(\dfrac{(q_{y}-p_{y,ijk})^{2}}{\sigma_{y,ijk}^{2}}\right)^{\beta}\right)}{1+exp(\alpha\dot{p}_{ijk}^{T}(q-p_{ijk}))}$

Equation 3 gives us the risk cost for all points $q$ in the planning region at
a given future time step $t_{i}$. We define
$\sigma_{ijk}=\dfrac{w_{ijk}}{2}+|\dot{p}_{ijk}|$ for the trajectory $k$ from
vehicle $j$ at time step $i$ accounting for the probability of each trajectory
and its time step of occurrence. From the risk of all points at the given time
we use Equation 1 to find the risk level set of points the ego vehicle can use
to plan for that time step. With this approach we seek to find the risk the
ego vehicle will experience at any point along the road over the next few
seconds. With this approach we can plan paths that are multiple seconds long
before determining which path to take. This is an important aspect of the
framework, since human drivers operate their vehicles by thinking about the
benefits or dangers or maneuvers over the timeframe of a few seconds. By
planning for trajectories that span multiple seconds we can replicate this
aspect of human driving and more select paths that imitate human driving
behavior.

###  III-C Path Planning and Navigation

After computing the risk at each future time step $t_{i}$ that is predicted
for each of the other agents we generate a grid of points or nodes spaced
$d_{p}$ along the length of the road $m=\\{1,...,N_{horiz}\\}$ and for each
lane on the road where $l=\\{1,...,N_{lanes}\\}$ with the position of each
node represented as $g_{ml}$. We connect the nodes of this graph with edges
that represent possible actions the car can take from any node. These actions
are: continue in the current lane, change to the left lane, or change to the
right lane. We assign weights $w_{mlc}$ according to Equation 4 to each edge
with $c={1,2,3}$ for each of the possible lane change actions, and at each
node we assign the corresponding level set $L_{\hat{P},i}$ to each point in
the planning grid $L_{ml}$ for if the risk at that point is lower than the
risk threshold $\mathcal{H}_{P}$.

$\displaystyle w_{mlc}=\mathcal{H}_{c,i}(g_{ml},p_{i},\dot{p}_{i})$ (4)

Using the planning grid, we employ Dijkstra’s Algorithm to determine the
lowest-overall-risk path for the ego vehicle similar to the approach from [7].
Dijkstra’s Algorithm is selected to guarantee that a globally optimal path is
found for the ego vehicle, and works efficiently for this algorithm due to the
relatively small number of nodes required for effective path for planning. At
each point, we verify if $L_{ml}$ falls within the planning level set
$L_{\hat{P},i}$ and incorporate it as a node in the planning graph only when
within this level set. If a complete path cannot be established from start to
finish, we select the longest path with the lowest cost. This approach
minimizes overall risk while avoiding exceeding a maximum safety threshold,
thus emulating various human behaviors that prioritize overall safety relative
to other vehicles on the road. Thresholding max risk with $L_{P,i}$ prevents
the vehicle from taking single overly risky maneuvers that humans would avoid,
even if they result in lower risk along the longer path, while optimizing for
the lowest-cost total path ensures the vehicle’s safety throughout.

Figure 2: The planning grid with nodes and edges for the ego vehicle (pictured
in blue) that is used to find a safe path through the surrounding agents.
Algorithm 1 Generate Multimodal Risk-Aware Planning Grid

1:for $\forall j\in\\{1,...,N_{agents}\\}$ do

2: Predict possible agent trajectories [25]

3:end for

4:Create graph in planning horizon length

5:for $\forall i\in\\{1,...,N_{time}\\}$ do

6: Compute risk $\mathcal{H}_{c,i}$ (Equation 3)

7:end for

8:for $\forall m\in\\{1,...,N_{horiz}\\}$ do

9: for $\forall l\in\\{1,...,N_{lanes}\\}$ do

10: Find time step $i$ for ego vehicle to reach edge $g_{ml}$

11: Update $w_{ml}$ for each edge (Equation 4)

12: Set $L_{ml}$ to the risk level set (Equation 1)

13: end for

14:end for

###  III-D Insights on Learned Parameters

To more accurately represent human driving behavior as opposed to simply
generating safe driving behavior we take the framework previously outlined and
learn additional costs that are used when planning trajectories. Not all lanes
along a highway travel at the same speed: human drivers prefer to drive in
different lanes based on their desired speed of travel. To reflect this
behavior in our model, we learn the driver’s lane preference based on the
current speed of the driver relative to the speed of other vehicles in their
lane.

To learn parameters as additional costs for the risk-aware planning, we divide
the NGSIM data from I-80 and US-101 in half, reserving half of the data to
validate our model. We also filter all data to find the smoothed positions and
velocities of all the vehicles to correct errors in the NGSIM data. Finally,
we re-extract the current lanes of each vehicle from its position to
accurately identify each vehicle’s lane at each time step in the NGSIM data.
The data used for training the model included approximately 4,500 cars with
over 80 hours of combined driving time for learning parameters to model the
driving on human behavior.

TABLE I: Learned Parameters for Model |  Lane 1 (Slowest) | Lane 2 | Lane 3 | Lane 4 |  Lane 5 (Fastest)
---|---|---|---|---|---
1 $\sigma$ slower |  0.4224 | 0.5030 | 0.5169 | 0.4994 |  0.4460
Mean |  0.4017 | 0.4868 | 0.4862 | 0.4677 |  0.4386
1 $\sigma$ faster |  0.4123 | 0.4962 | 0.5070 | 0.4891 |  0.4421

Figure 3: A visualization of the risk in each lane relative to the velocity of
the vehicle. This is a visualization of data seen in Table I.

For each time step of each vehicle in the data, we compute the risk field at
the current time step $t_{i}$ using the rectangular cost function in Equation
2, and record the risk the vehicle is experiencing, along with its velocity,
current lane, and the mean and standard deviation of velocity of all vehicles
in the current vehicle’s lane. We process these data to determine the average
risk a vehicle experiences when traveling within the range of 1.5 standard
deviations from the mean velocity of the lane $[-1.5\sigma,1.5\sigma]$. We
selected $1.5\sigma$ since about $90\%$ of all vehicle velocities fall within
this range, leading to a good fit for the data without giving too much weight
to outliers. The data can be found in Table I (see Figure 3 for a
visualization of these data).

To incorporate the learned parameters into our model we modify the weights of
the edges of the planning grid with Equation 6 to reflect the difference in
risk that the vehicle would experience by changing lane. We fit a function to
the acquired data $\mathcal{H}_{l}(l,u_{avg},u_{\sigma},u_{ijk})$ represented
by Equation 5 with an input of the current current lane, average lane
velocity, standard deviation of lane velocity and the velocity of a vehicle
and output of the average risk in the lane bounded in the range $[0,1]$. Then
we compute the current value of $\Delta\mathcal{H}_{l}$ using Equation 5.

$\displaystyle\Delta\mathcal{H}_{l}=$
$\displaystyle\mathcal{H}_{l}(l2,u_{l2,avg},u_{l2,\sigma},u_{i+1jk})$ (5)
$\displaystyle-\mathcal{H}_{l}(l1,u_{l1,avg},u_{l1,\sigma},u_{ijk})$

$\displaystyle
w_{mlc}=\mathcal{H}_{c,i}(g_{ml},p_{ijk},\dot{p}_{ijk})+\Delta\mathcal{H}_{l}$
(6)

13:05:31 202 Using Equation 6 as the new cost function for the edges of the
planning grid in Algorithm 1 generates two new behaviors in the ego vehicle
that replicate human driving behavior in highway driving. First, it rewards
the ego vehicle for traveling at the speed of the other traffic in the lane.
As the ego vehicle speeds up or slows down to reach the same velocity as the
other vehicles in the lane the $\Delta\mathcal{H}_{l}$ factor decreases, which
lowers the total cost of the path. This behavior of speeding up or slowing
down in a lane to match the average speed of other cars in the lane more
closely matches human behavior when driving within one lane on a highway as
opposed to constantly speeding up and slowing down to maintain a safe distance
other vehicles.

Second, the $\Delta\mathcal{H}_{l}$ factor encourages the ego vehicle to enter
lanes that align with the ego vehicle’s desired speed. We input the ego
vehicle’s desired velocity as a planner parameter, which guides it to lanes
with velocities matching its preference. When the ego vehicle aims to surpass
its current lane’s speed, it shifts to a faster lane, lowering its
$\Delta\mathcal{H}_{l}$ term. Conversely, it changes to a slower lane when
desiring a lower speed than the current lane’s average velocity. These
emergent behaviors, arising from this single cost term addition, highlight the
approach’s advantages. Many such terms can be learned from data to improve the
modeling of human behaviors in varying driving situations. In the next
section, we validate our model’s lane change prediction ability using vehicle
data and demonstrate its effective use in replicating human driver lane
changes. The process for learning parameters can be easily repeated for new
parameters, continually generating accurate human behaviors for various
driving scenarios.

##  IV Simulation and Discussion

###  IV-A Model validation

In this section, we validate our model using real-world data and show specific
examples of our framework generating human-like behavior not found in other
simulation methods for human drivers. We validate the model using the
approximately 1,000 lane change maneuvers in the half of the NGSIM data that
we did not use to learn the parameters. Further, we subdivide the data by
examining the lateral velocities of each driver during their maneuvers and
categorizing the drivers as conservative, normal, or aggressive. We define
drivers with the lower quartile of lateral and longitudinal accelerations
during lane changes as conservative, drivers in the upper quartile as
aggressive, and the rest as normal. We define three different parameters for
aggressiveness that represent the varying driving styles, and validate each
lane change maneuver with the corresponding model.

To perform the validation, we replace one of the recorded vehicles from NGSIM
data in the simulation with our model 2 seconds before the lane change occurs
in the data, and observe the planned trajectory for the next 4 seconds to
determine if the model changes lanes into the same lane as the actual data. We
implement the previously described method of planning with a model predictive
controller and record the final and average deviation of our model from the
ground truth. We divide the error into lateral, and longitudinal error to
demonstrate the models performance in both directions. We present our results
in Table II.

TABLE II: Driver Model Validation by Driver Type |  Aggressive |  Normal |  Conservative |  Total
---|---|---|---|---
Accuracy |  77.41% |  81.27% |  83.73% |  80.92%
RMSElat,avg |  0.348 m |  0.312 m |  0.285 m |  0.315 m
RMSElat,final |  0.518 m |  0.436 m |  0.384 m |  0.446 m
RMSElon,avg |  3.51 m |  3.12 m |  2.98 m |  3.20 m
RMSElon,final |  5.32 m |  5.11 m |  5.01 m |  5.15 m

Table II demonstrates our model’s capability to accurately predict lane
changes. Notably, prediction accuracy decreases slightly as driver
aggressiveness increases. This is likely due to the more variable nature of
aggressive driving behaviors. Aggressive drivers often prioritize speed as-
opposed-to safety and adherence to traffic rules, expanding the set of
potential actions beyond those of conservative drivers. This variability makes
it challenging to pinpoint the exact actions of aggressive drivers, as
reflected in our results. Importantly, the slight decrease in accuracy for
aggressive drivers does not hinder our model’s capability to generate
plausible aggressive driving maneuvers. The framework may select a slightly
different, but realistic aggressive maneuver within the broader range of
possibilities.

TABLE III: Comparison of existing models for predicting lane change maneuvers with 3 seconds of prediction horizon Model | Dataset | Accuracy
---|---|---
Proposed | NGSIM | 81%
Support-vector Machine [26] | NGSIM | 47%
Multi-Layer Perceptrons [26] | NGSIM | 58%
Random Forest [26] | NGSIM | 63%
LSTM Network [27] | NGSIM | 86%
Convolutional Neural Network [26] | NGSIM | 91%

We provide a comparison of our model’s ability to predict lane changes by
human drivers in Table III. While the Convolutional Neural Network and LSTM
Network yield slightly better results in lane prediction than our proposed
model, we assert that our model remains valid as a method for modeling human
driving behavior as our model prioritizes generating new human-like
trajectories over precise lane change predictions. Furthermore, we
comprehensively model the entire highway driving scenario, providing insights
into both the likelihood and location of lane changes, in contrast to the
binary classification of lane change occurrence offered by many other methods.
We additionally emphasize the uniqueness of our approach in modeling human
driving behavior based on adjustable hyperparameters for aggressiveness. These
parameters enable our model to generate multiple distinct human-like
trajectories for the same initial highway scenario leading to an improved
capability of our model to be used for testing autonomous vehicle frameworks.

We acknowledge that training exclusively on NGSIM data restricts the model’s
ability to reproduce driving maneuvers absent from the dataset. However,
NGSIM’s diverse range of drivers suggests that, with sufficient data on other
driving maneuvers, this approach can learn additional behaviors.

###  IV-B Example Cases

To demonstrate the ability of this model to generate new human behaviors not
seen in data we present the following scenarios which showcase our framework’s
ability to generate diverse behaviors. All scenarios shown below are
sequential snapshots of egocentric views of a simulated highway environment
with the ego vehicle is depicted in dark blue and the planned trajectory shown
by the pink line.

####  IV-B1 Weaving through traffic

t = 0.0s

t = 0.7s

t = 5.6s

t = 15.7s

t = 17.7s

Figure 4: A demonstration of the model’s ability to weave through traffic. The
ego vehicle starts behind all vehicles on the road, but with a higher velocity
and risk threshold $\mathcal{H}_{P}$ than all surrounding vehicles. The risk-
aware planner navigates a safe human-like route to weave through the traffic.
Video

In Figure 4, the ego vehicle, with a higher desired velocity and risk
threshold $\mathcal{H}_{P}$, displays more aggressive behavior compared to its
surroundings, weaving through traffic to emulate aggressive driving. Current
human behavior models struggle in weaving through dense traffic due to two
main limitations. Firstly, models with short planning horizons lack sufficient
time to plan a path through dense traffic, causing the ego vehicle to follow
dense traffic groups rather than weaving through them. Secondly, when existing
models attempt weaving, they frequently collide with surrounding vehicles due
to a lack of consideration for the multimodal nature of surrounding vehicle
trajectories which leads to high levels of uncertainty in the actions of other
vehicles. Our approach, with its long planning horizon and multimodal
trajectory prediction, safely navigates traffic, accurately representing
aggressive driving behavior.

####  IV-B2 Moving out of way of high-speed vehicle

In Figure 5, the ego vehicle, with a lower desired velocity and risk threshold
$\mathcal{H}_{P}$, exhibits a more cautious approach than surrounding vehicles
as it encounters a fast-approaching car from behind. Our Risk-Aware planning
framework guides the ego vehicle to change lanes, allowing the faster vehicle
to pass. This behavior accurately models a conservative driver prioritizing
safe driving by making way for a faster driver. Most current driver models
lack this behavior due to inadequate forward-looking risk assessments for
surrounding vehicles. Consequently, they fail to predict potential risks from
other vehicles early enough, resulting in a keep-lane trajectory that causes
the following vehicle to decelerate, even when a conservative human driver
would change lanes. With our forward-looking risk assessment, the ego vehicle
predicts the risk posed by the approaching vehicle well in advance, enabling
it to yield the lane to the faster vehicle, mirroring the behavior of a
cautious human driver.

t = 0.0s

t = 0.4s

t = 2.0s

t = 6.4s

Figure 5: A demonstration of the model’s ability to move out of the way of an
approaching high-speed vehicle to maintain safety. The ego vehicle can be seen
starting in front of a high-speed vehicle and traveling at a slower velocity
and risk threshold $\mathcal{H}_{P}$ than the approaching vehicle. The planner
navigates a safe human-like route to move out of the way. Video

##  V Conclusion

In this paper, we propose the hierarchical learned risk-aware planning
framework for modeling human drivers and validate and demonstrate its
effectiveness in replicating human behavior. We additionally showcase the
model’s ability to generate human-like driving patterns in diverse, unseen
situations. Future work will involve enhancing the model to encompass
different driving characteristics [28, 29], such as distracted driving,
broadening the range of drivers it can represent. We also plan to use methods
like probing [30] to actively learn additional parameters to improve the
models ability to mimic human behavior across a wider array of situations and
conduct further validation in diverse contexts to confirm its generalizability
to other driving scenarios.

## References

  * [1] Yiwei Lyu, Chiyu Dong and John M Dolan  “FG-GMM-based interactive behavior estimation for autonomous driving vehicles in ramp merging control”  In _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 1250–1255  IEEE
  * [2] U.S. Department Of Transportation Federal Highway Administration  “Next Generation Simulation (NGSIM) Vehicle Trajectories and Supporting Data”  Not Available, 2017  DOI: 10.21949/1504477
  * [3] Martin Treiber, Ansgar Hennecke and Dirk Helbing  “Congested traffic states in empirical observations and microscopic simulations”  In _Physical Review E_ 62.2, 2000, pp. 1805–1824  DOI: 10.1103/PhysRevE.62.1805
  * [4] Jaime F. Fisac et al.  “Hierarchical Game-Theoretic Planning for Autonomous Vehicles” arXiv:1810.05766 [cs, math]  arXiv, 2018  URL: http://arxiv.org/abs/1810.05766
  * [5] Hongtao Yu, H. Tseng and Reza Langari  “A human-like game theory-based controller for automatic lane changing”  In _Transportation Research Part C: Emerging Technologies_ 88, 2018, pp. 140–158  DOI: 10.1016/j.trc.2018.01.016
  * [6] Alyssa Pierson, Wilko Schwarting, Sertac Karaman and Daniela Rus  “Navigating Congested Environments with Risk Level Sets”  In _2018 IEEE International Conference on Robotics and Automation (ICRA)_ Brisbane, QLD: IEEE, 2018, pp. 5712–5719  DOI: 10.1109/ICRA.2018.8460697
  * [7] Alyssa Pierson, Wilko Schwarting, Sertac Karaman and Daniela Rus  “Learning Risk Level Set Parameters from Data Sets for Safer Driving”  In _2019 IEEE Intelligent Vehicles Symposium (IV)_ Paris, France: IEEE, 2019, pp. 273–280  DOI: 10.1109/IVS.2019.8813842
  * [8] Hafiz Usman Ahmed, Ying Huang and Pan Lu  “A Review of Car-Following Models and Modeling Tools for Human and Autonomous-Ready Driving Behaviors in Micro-Simulation”  In _Smart Cities_ 4.1, 2021, pp. 314–335  DOI: 10.3390/smartcities4010019
  * [9] Dorsa Sadigh, Shankar Sastry, Sanjit A. and Anca D.  “Planning for Autonomous Cars that Leverage Effects on Human Actions”  In _Robotics: Science and Systems XII_ Robotics: ScienceSystems Foundation, 2016  DOI: 10.15607/RSS.2016.XII.029
  * [10] Dorsa Sadigh, S. Sastry and Sanjit A. Seshia  “Verifying Robustness of Human-Aware Autonomous Cars”  In _IFAC-PapersOnLine_ 51.34, 2019, pp. 131–138  DOI: 10.1016/j.ifacol.2019.01.055
  * [11] Zhangjie Cao et al.  “Reinforcement Learning based Control of Imitative Policies for Near-Accident Driving” arXiv:2007.00178 [cs, eess, stat]  arXiv, 2020  URL: http://arxiv.org/abs/2007.00178
  * [12] Alex Kuefler, Jeremy Morton, Tim Wheeler and Mykel Kochenderfer  “Imitating driver behavior with generative adversarial networks”  In _2017 IEEE Intelligent Vehicles Symposium (IV)_ Los Angeles, CA, USA: IEEE, 2017, pp. 204–211  DOI: 10.1109/IVS.2017.7995721
  * [13] Felipe Codevilla, Eder Santana, Antonio M. López and Adrien Gaidon  “Exploring the Limitations of Behavior Cloning for Autonomous Driving” arXiv:1904.08980 [cs]  arXiv, 2019  URL: http://arxiv.org/abs/1904.08980
  * [14] Olger Siebinga, Arkady Zgonnikov and David Abbink  “A Human Factors Approach to Validating Driver Models for Interaction-aware Automated Vehicles”  In _ACM Transactions on Human-Robot Interaction_ 11.4, 2022, pp. 1–21  DOI: 10.1145/3538705
  * [15] Yiwei Lyu, Wenhao Luo and John M Dolan  “Adaptive safe merging control for heterogeneous autonomous vehicles using parametric control barrier functions”  In _2022 IEEE Intelligent Vehicles Symposium (IV)_ , 2022, pp. 542–547  IEEE
  * [16] Guofa Li, Shengbo Eben Li, Bo Cheng and Paul Green  “Estimation of driving style in naturalistic highway traffic using maneuver transition probabilities”  In _Transportation Research Part C: Emerging Technologies_ 74, 2017, pp. 113–125  DOI: 10.1016/j.trc.2016.11.011
  * [17] Jianqiang Wang et al.  “Driving risk assessment using near-crash database through data mining of tree-based model”  In _Accident Analysis & Prevention_ 84, 2015, pp. 54–64  DOI: 10.1016/j.aap.2015.07.007
  * [18] Thomas A. Dingus et al.  “Driver crash risk factors and prevalence evaluation using naturalistic driving data”  In _Proceedings of the National Academy of Sciences_ 113.10, 2016, pp. 2636–2641  DOI: 10.1073/pnas.1513271113
  * [19] Yiwei Lyu, Wenhao Luo and John M Dolan  “Risk-aware Safe Control for Decentralized Multi-agent Systems via Dynamic Responsibility Allocation”  In _2023 IEEE International Conference on Intelligent Robots and Systems (IROS)_ , 2023  IEEE
  * [20] Yiwei Lyu, John M Dolan and Wenhao Luo  “Decentralized Safe Navigation for Multi-agent Systems via Risk-aware Weighted Buffered Voronoi Cells”  In _Proceedings of the 2023 International Conference on Autonomous Agents and Multiagent Systems_ , 2023, pp. 1476–1484
  * [21] Yiwei Lyu, John M Dolan and Wenhao Luo  “CBF-inspired Weighted Buffered Voronoi Cells for Distributed Multi-agent Collision Avoidance”  In _2023 American Control Conference (ACC)_ , 2023, pp. 4513–4518  IEEE
  * [22] Marcello Montanino and Vincenzo Punzo  “Making NGSIM Data Usable for Studies on Traffic Flow Theory: Multistep Method for Vehicle Trajectory Reconstruction”  In _Transportation Research Record: Journal of the Transportation Research Board_ 2390.1, 2013, pp. 99–111  DOI: 10.3141/2390-11
  * [23] Jeremy Morton, Tim A. Wheeler and Mykel J. Kochenderfer  “Analysis of Recurrent Neural Networks for Probabilistic Modeling of Driver Behavior”  In _IEEE Transactions on Intelligent Transportation Systems_ 18.5, 2017, pp. 1289–1298  DOI: 10.1109/TITS.2016.2603007
  * [24] Christian Thiemann, Martin Treiber and Arne Kesting  “Estimating Acceleration and Lane-Changing Dynamics from Next Generation Simulation Trajectory Data”  In _Transportation Research Record: Journal of the Transportation Research Board_ 2088.1, 2008, pp. 90–101  DOI: 10.3141/2088-10
  * [25] Nachiket Deo and Mohan M. Trivedi  “Convolutional Social Pooling for Vehicle Trajectory Prediction” arXiv:1805.06771 [cs]  arXiv, 2018  URL: http://arxiv.org/abs/1805.06771
  * [26] Yi Zhang, Sheng Zhang and Ruikang Luo  “Lane Change Intent Prediction Based on Multi-Channel CNN Considering Vehicle Time-Series Trajectory”  In _2022 IEEE 25th International Conference on Intelligent Transportation Systems (ITSC)_ Macau, China: IEEE, 2022, pp. 287–292  DOI: 10.1109/ITSC55140.2022.9921941
  * [27] Shuang Su et al.  “Learning Vehicle Cooperative Lane-changing Behavior from Observed Trajectories in the NGSIM Dataset”  In _Proceedings of IEEE Intelligent Vehicles Symposium (IV ’18)_ , 2018, pp. 1412–1417
  * [28] Yiwei Lyu, Wenhao Luo and John M Dolan  “Probabilistic safety-assured adaptive merging control for autonomous vehicles”  In _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 10764–10770  IEEE
  * [29] Yiwei Lyu, Wenhao Luo and John M Dolan  “Responsibility-associated Multi-agent Collision Avoidance with Social Preferences”  In _2022 IEEE 25th International Conference on Intelligent Transportation Systems (ITSC)_ , 2022, pp. 3645–3651  IEEE
  * [30] Shuangge Wang, Yiwei Lyu and John M. Dolan  “Active Probing and Influencing Human Behaviors Via Autonomous Agents” arXiv:2304.11817 [cs]  arXiv, 2023  URL: http://arxiv.org/abs/2304.11817