Norwegian University of Life Sciences

Faculty of Science and Technology

Philosophiae Doctor (PhD)

Thesis 2024:23

Traversability-guided Planning
for Efficient Robot Navigation
in Uneven Terrains

Traversibilitetsstyrt planlegging for

effektiv robotnavigasjon i ulendt terreng

Fetullah Atas
 Traversability-guided Planning for Efficient
Robot Navigation in Uneven Terrains

Traversibilitetsstyrt planlegging for effektiv robotnavigasjon i ulendt

terreng

Philosophiae Doctor (PhD) Thesis

Fetullah Atas

Norwegian University of Life Sciences

Faculty of Science and Technology

Ås 2024

Thesis number 2024:23

ISSN 1894-6402
ISBN 978-82-575-2145-5
“What I love about science is that as you learn, you don’t really get answers.
You just get better questions.” - John Green
Abstract
This doctoral thesis presents several novel contributions to robot navigation
in uneven terrains, emphasizing the development of innovative approaches in
motion planning, environmental representation, traversability estimation, and
kinodynamic motion planning. The research focuses on the critical importance
of effectively navigating uneven terrains, a challenge particularly pertinent to
outdoor robots operating under strict dynamic constraints such as steering,
velocity, and acceleration limits.
A notable contribution of this thesis is the integration of terrain information
exploitation for optimal motion planning in challenging environments. Breaking
new ground beyond traditional geometric motion planning, the work delves
deep into kinodynamic motion planning, addressing the reduced maneuverability
caused by dynamic constraints (e.g., velocity or torque) and enhancing the
safety and efficacy of outdoor robotic systems. Another key innovation in this
thesis is the development of a neural network-based dynamic traversability
estimation method. This approach is tailored to the robot’s unique capabilities
and orientation, leveraging its navigational experiences for learning. Utilization
of these traversability estimates results in more precise and reliable collision-free
motion plans and effective control mechanisms, significantly improving the safety
and efficiency of complex navigation tasks.
Furthermore, the thesis introduces a comprehensive navigation framework
with a standardized, plugin-based interface. This framework facilitates the
integration of new algorithms and ensures seamless interoperability among
different components, such as controllers, motion planners, and environmental
collision checkers. This holistic approach validates our proposed methodologies’
effectiveness in real-world scenarios and lays the groundwork for future research
in robotic navigation through complex and uneven terrains.
Overall, this thesis contributes to the field of robotic navigation by
offering novel insights, methodologies, and a versatile framework that enhances
adaptability, precision, and responsiveness. The experiments and validation of the
algorithms presented in this thesis were conducted using two distinct real robot
platforms. These platforms were tested across a diverse range of environments,
ensuring a comprehensive evaluation of the algorithms’ performance and
applicability in various real-world scenarios.

iii
Sammendrag
Denne doktorgradsavhandlingen presenterer flere nye bidrag til robotnavigasjon
i ujevnt terreng, og legger vekt på utviklingen av innovative tilnærminger
innen bevegelsesplanlegging, miljørepresentasjon, estimering av kryssbarhet
og kinodynamisk bevegelsesplanlegging. Forskningen fokuserer på den kritiske
viktigheten av å effektivt navigere i ujevnt terreng, en utfordring spesielt relevant
for utendørsroboter som opererer under strenge dynamiske begrensninger som
styrings-, hastighets- og akselerasjonsgrenser.
Et bemerkelsesverdig bidrag fra denne avhandlingen er utnyttelse av
terrenginformasjon for optimal bevegelsesplanlegging i utfordrende miljøer.
Arbeidet bryter ny mark utover tradisjonell geometrisk bevegelsesplanlegging,
og går dypt inn i kinodynamisk bevegelsesplanlegging, og adresserer den
reduserte manøvrerbarheten forårsaket av dynamiske begrensninger (f.eks.
hastighet eller dreiemoment) og forbedrer sikkerheten og effektiviteten til
utendørs robotsystemer. En annen nøkkelinnovasjon i denne oppgaven, er
utviklingen av en dynamisk traversabilitetsstimeringsmetode basert på nevrale
nettverk. Denne tilnærmingen er skreddersydd til robotens unike evner og
orientering, og utnytter dens navigasjonsopplevelser for læring. Utnyttelse av
disse traversibilitetsestimatene resulterer i mer presise og pålitelige kollisjonsfrie
bevegelsesplaner og effektive kontrollmekanismer, noe som betydelig forbedrer
sikkerheten og effektiviteten til komplekse navigasjonsoppgaver.
Videre introduserer oppgaven et omfattende navigasjonsrammeverk med et
standardisert, plugin-basert grensesnitt. Dette rammeverket letter integreringen
av nye algoritmer og sikrer sømløs interoperabilitet mellom forskjellige kom-
ponenter, for eksempel kontrollere, bevegelsesplanleggere og kollisjonssjekkere.
Denne helhetlige tilnærmingen validerer de foreslåtte metodikkenes effektivitet
i scenarier i den virkelige verden og legger grunnlaget for fremtidig forskning
innen robotnavigasjon gjennom komplekse og ujevnt terreng.
Samlet sett bidrar denne avhandlingen til feltet robotnavigasjon ved å tilby
ny innsikt, metoder og et allsidig rammeverk som forbedrer tilpasningsevne,
presisjon og respons. Eksperimentene og valideringen av algoritmene presentert
i denne oppgaven ble utført ved hjelp av to distinkte robotplattformer. Disse
plattformene ble testet på tvers av et mangfold av miljøer, og sikret en omfattende
evaluering av algoritmenes ytelse og anvendelighet i ulike scenarier i den virkelige
verden.

v
Preface
This thesis is hereby submitted in partial fulfillment of the requirements for
the degree of Philosophiae Doctor (Ph.D.) at the Norwegian University of
Life Sciences (NMBU). The research presented was undertaken at the NMBU
Robotics and Control Lab, a constituent of the Faculty of Science and Technology
(REALTEK). Spanning from December 2020 to December 2023, this research
period was marked by studies and experimental work, all aimed at contributing
to the advancement of robot navigation techniques in uneven terrains.
This thesis is systematically organized into several chapters. It starts with
an introductory chapter that sets the stage for the research, followed by a
background chapter that lays the context of the thesis. The subsequent sections
are dedicated to explaining the methodologies employed throughout the research.
This is followed by a summary of six research papers that constitute the core of
the thesis. The majority of these papers have undergone a stringent peer-review
process and have been published, with two papers currently under review as of the
writing of this thesis. In the discussion chapter, there is a critical evaluation of
the research findings, where limitations are acknowledged, and potential avenues
for future research are identified. The final chapter encapsulates concluding
remarks and outlines promising future work, building upon the methodologies
and discoveries presented in this thesis.

vii
Acknowledgements
I extend my profound gratitude to my supervisors, Grzegorz Cielniak and
Lars Grimstad, for their invaluable guidance and support. Their expertise and
insightful perspectives have been pivotal in shaping the contributions of this
thesis. The enlightening discussions and productive meetings we shared over the
past three years have not only inspired me but also played a crucial role in the
fruition of my research. Their mentorship has been a significant factor in my
academic journey, for which I am deeply thankful. A heartfelt thanks to both of
you.
I wish to express my deepest gratitude to my family, whose unwavering belief
in me was a beacon of hope, especially during moments when challenges seemed
insurmountable. Their enduring support and faith have been fundamental to
my perseverance and success.
Additionally, I extend my sincere thanks to all my colleagues at NMBU
Robotics Lab. for their assistance and camaraderie throughout this journey.
Their contributions, both big and small, have been invaluable in facilitating my
research and enriching my academic experience.
Last but not least, I wish to express my profound love for the mountains
of Kurdistan, the place where I grew up and to which I am deeply connected.
These majestic landscapes have taught me the essence of resilience and courage,
shaping the person I have become. Their enduring strength and beauty have
been a constant source of inspiration and guidance throughout my journey.

Fetullah Atas
February, 2024

ix
List of Papers
Paper I
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad. “Benchmark of Sampling-Based
Optimizing Planners for Outdoor Robot Navigation”. In: Intelligent Autonomous
Systems 17 (IAS). Vol. 577, June 2022, DOI: 10.1007/978-3-031-22216-0_16.

Paper II
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad “Elevation State-Space: Surfel-
Based Navigation in Uneven Environments for Mobile Robots” In: IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), 2022, DOI:
10.1109/IROS47612.2022.9981647

Paper III
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad “Navigating in 3D Uneven
Environments Through Supervoxels and Nonlinear MPC” In: European
Conference on Mobile Robots (ECMR) DOI: 10.1109/ECMR59166.2023.10256342

Paper IV
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad “CostTrust: A Fast-Exploring,
Iteratively Expanding Frontier-Based Kinodynamic Motion Planner” Submitted
for Journal of Modeling, Identification and Control (MIC), 2024.

Paper V
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad “From Simulation to Field:
Learning Terrain Traversability for Real-World Deployment” Submitted for
Robotics and Autonomous System (RAS) Journal, 2023.

Paper VI
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad “Enabling Robot Autonomy
through a Modular Software Framework” In: ICRA Workshop on Robotic
Software Architectures, 2023.

xi
Contents

Abstract iii

Sammendrag v

Preface vii

Acknowledgements ix

List of Papers xi

Contents xiii

List of Figures xvii

1 Introduction 1
1.1 Graphical Abstract . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 Research Questions . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.5 The Papers and Thesis Outline . . . . . . . . . . . . . . . 6

2 Background 9
2.1 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Environmental Representation . . . . . . . . . . . . . . . . 14
2.3 Traversablity Estimation . . . . . . . . . . . . . . . . . . . 16
2.4 Emerging Approaches in Uneven Terrain Navigation . . . . 18

3 Methodological Framework 21
3.1 Explanation of Research Design . . . . . . . . . . . . . . . 21
3.2 Robot Platforms and Data . . . . . . . . . . . . . . . . . . 21
3.3 Environments and Simulations . . . . . . . . . . . . . . . . 23
3.4 Application Scenarios . . . . . . . . . . . . . . . . . . . . . 24
3.4.1 Precision Agriculture on Hilly Farms . . . . . . . 24
3.4.2 Dense Forest Exploration . . . . . . . . . . . . . 24
3.4.3 Delivery Robots in Scarcely Populated Towns . . 24
3.5 Software Tools . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.6 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

4 Paper Summaries 27

xiii
Contents

4.1 Paper I: Benchmark of Sampling-Based Optimizing Planners

for Outdoor Robot Navigation . . . . . . . . . . . . . . . . 27
4.2 Paper II: Elevation State-Space: Surfel-Based Navigation in
Uneven Environments for Mobile Robots . . . . . . . . . . 28
4.3 Paper III: Navigating in 3D Uneven Environments Through
Supervoxels and Nonlinear MPC . . . . . . . . . . . . . . . 28
4.4 Paper IV: CostTrust: A Fast-Exploring, Iteratively Expand-
ing Frontier-Based Kinodynamic Motion Planner . . . . . 29
4.5 Paper V: From Simulation to Field: Learning Terrain
Traversability for Real-World Deployment . . . . . . . . . 30
4.6 Paper VI: Enabling Robot Autonomy through a Modular
Software Framework . . . . . . . . . . . . . . . . . . . . . . 31

5 Discussion 33
5.1 The Motion Planning Problem in Outdoor Uneven Terrains 33
5.2 The Need for Exploitation of Terrain . . . . . . . . . . . . 33
5.3 The Adaptability of Different Robot Platforms to Uneven
Terrains . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
5.4 Kinodynamic Motion Planning for Uneven Terrains . . . . 34
5.5 Going Beyond Geometrical or Visual-based Traversability
Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
5.6 A Holistic Approach to Uneven Terrain Navigation . . . . 35

6 Conclusion and Future Work 37

6.1 Future Directions . . . . . . . . . . . . . . . . . . . . . . . 38
6.1.1 The Terrain-Robot Reaction Estimation . . . . . 38
6.1.2 Advanced Sensory Fusion for Terrain-Robot Inter-
action Analysis . . . . . . . . . . . . . . . . . . . 38
6.1.3 Increasing the Adaptability to Dynamic Environ-
ments . . . . . . . . . . . . . . . . . . . . . . . . 38
6.1.4 A Push Towards Mapless Navigation . . . . . . . 39
6.1.5 Development of Reactive Navigation Systems
Based on Real-Time Environmental Feedback . . 39
6.1.6 Hybrid Navigation Models Combining Rule-Based
and Machine Learning Approaches . . . . . . . . 39

Papers 42

I Benchmark of Sampling-Based Optimizing Planners for

Outdoor Robot Navigation 43

II Elevation State-Space: Surfel-Based Navigation in Uneven

Environments for Mobile Robots 59

III Navigating in 3D Uneven Environments Through Super-

voxels and Nonlinear MPC 69

xiv
 Contents

IV CostTrust: A Fast-Exploring, Iteratively Expanding

Frontier-Based Kinodynamic Motion Planner 79

V From Simulation to Field: Learning Terrain Traversability

for Real-World Deployment 95

VI Enabling Robot Autonomy through a Modular Software

Framework 135

Bibliography 141

xv
List of Figures

1.1 Synthesis of Uneven Terrain Navigation Research – This figure

encapsulates the comprehensive contributions of the papers that
construct this thesis, detailing advancements in motion planning,
kinodynamic motion planning, environmental representations, and
traversability estimation. It highlights the papers associated with
each domain and outlines their key achievements, such as the
development of novel algorithms, methodologies, and integration
into established libraries, all aimed at enhancing the autonomy
and navigational capabilities of robots in complex terrains. . . . 2

1.2 The figure illustrates the Thorvald II [1] robot’s capability to

operate in various rough uneven terrains. To fully utilize the
robot’s strengths in these terrains, it is crucial to have a reliable
navigation strategy. . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 The figure provides a systematic enumeration of the papers,

detailing the specific research questions they address, and outlining
the precise context and focus of each paper. . . . . . . . . . . . . 7

2.1 The figure illustrates the working principles of the Probabilistic

RoadMap (PRM) [2], a widely recognized sampling-based motion
planning approach. . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.2 This figure illustrates the Rapidly-Exploring Random Tree (RRT)

algorithm, which employs a tree structure to systematically search
the state space and establish a path to the desired goal state. The
inherent structure of RRT, based on tree construction, renders
it less suitable for re-planning scenarios, as a new tree must be
generated for each new set of start and goal pairs. . . . . . . . . 11

2.3 This figure depicts kinodynamic motion planning using a modified

RRT algorithm. Each new sample must conform to the
system’s motion constraints in this process, often achieved
through steering functions or random forward propagation with
varied control inputs. While this restricts maneuverability, it’s
crucial for ensuring smooth and feasible motion plans. In the
realm of kinodynamic planning, the paramount concern is the
strict adherence to both physical constraints and control input
limitations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

xvii
List of Figures

2.4 Illustrated are various environmental representations for uneven

terrain navigation: a. 2D occupancy grid, b. DEM, c. 3D
OctoMap, d. 3D Mesh, e. VDB-grid, and f. raw point clouds.
This thesis highlights the importance of selecting an appropriate
environmental representation based on the robot’s operational
environment and explores the integration of these representations
with motion planning and control. The focus is on enhancing
motion planning effectiveness through raw point cloud-based
methods, thereby improving navigation plans for robots in complex
terrains. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.5 Diverse traversability estimation methods for varied environments
are depicted. (a) Bayesian Generalized Kernel-based traversability
estimates applied to Digital Elevation Maps (DEM)[3], (b) image-
based terrain classification[4], (c) obstacle proximity-based 2D
costmaps[5], and (d) our contribution of a deep neural network-
based traversability estimation on 3D point clouds. . . . . . . . 17

3.1 The figure displays two robotic platforms: the Thorvald II robot
on the left and the Robotnik platform on the right. Both
platforms have a consistent sensor suite comprising LIDAR,
Inertial Measurement Units (IMUs), wheel encoders, and GNSS
receivers. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
3.2 Sensors mounted on a robot are depicted on the left. The robot’s
pose is determined by combining multiple sensory data using an
Extended Kalman Filter (EKF). The dark red points are from the
live LIDAR sensor that precisely overlaps the point cloud map,
indicating precise localization. . . . . . . . . . . . . . . . . . . . 22
3.3 This figure presents a collection of satellite imagery and first-
person photographs of the testing sites. . . . . . . . . . . . . . . 23

4.1 This figure presents the components constructing the approach in

Paper V. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

xviii
Chapter 1

Introduction
Autonomous robot navigation across uneven terrains holds significant importance,
with wide-ranging applications including exploration, mining, search and rescue,
agriculture, and forestry. While autonomous navigation within two-dimensional
environments is well-established with standard techniques based on 2D occupancy
grids, navigating outdoor uneven terrains introduces a spectrum of complex
challenges that necessitate further research. These challenges encompass the
requirement for the nontrivial robust terrain traversability analysis, which is
essential to prevent stability issues like tipping over or getting stuck. Additionally,
efficient motion planning is crucial to avoid collisions, and adaptability to dynamic
and changing environments is imperative. A further complication arises from the
fact that different robotic platforms possess varying traversability capabilities,
influenced by their specific design and locomotion mechanisms. Addressing these
multifaceted challenges is critical for advancing the field of robotic navigation in
complex outdoor environments.
This thesis presents a series of contributions designed to address a range of
problems pertinent to enhancing the autonomy of robots operating in uneven
terrains. The scope of our research encompasses various key areas, including
motion planning, terrain traversability analysis, and efficient environmental
representation techniques. Each of these areas is crucial for developing more
advanced and autonomous robotic systems capable of effectively navigating and
performing tasks in complex, uneven landscapes. Through our comprehensive
approach, we aim to significantly advance state-of-the-art robotic navigation
and control in challenging outdoor environments.

1.1 Graphical Abstract

The inclusion of a graphical abstract is vital to effectively highlight the collective
contributions of this thesis, which are encapsulated within six scholarly articles
presented at conferences and published in journals. These papers collaboratively
construct the research framework, each delving into the intricate navigation
challenges faced by outdoor robots traversing uneven terrains. Figure 1.1 offers
a visual synthesis of these six papers, spotlighting their principal contributions.
It is essential to underscore the aims, motivations, and integral roles these
papers play within the broader context of uneven terrain navigation—a recurring
theme that is intricately touched upon throughout the chapters and sections of
this thesis. Thus, the visual abstract in section 1.1 is designed to contextually
anchor the six papers, providing a cohesive overview that enhances the reader’s
comprehension of the research trajectory and its underlying objectives.

1
1. Introduction

Figure 1.1: Synthesis of Uneven Terrain Navigation Research – This figure

encapsulates the comprehensive contributions of the papers that construct this
thesis, detailing advancements in motion planning, kinodynamic motion planning,
environmental representations, and traversability estimation. It highlights the
papers associated with each domain and outlines their key achievements, such
as the development of novel algorithms, methodologies, and integration into
established libraries, all aimed at enhancing the autonomy and navigational
capabilities of robots in complex terrains.

2
 Motivation

1.2 Motivation
This thesis addresses the complex challenges faced by mobile robots when
navigating autonomously in outdoor environments. While most of these
environments are not entirely flat, the variations in elevation are often not
drastic, allowing the assumption of a 2D environmental representation to suffice
in many scenarios. For instance, the 2D-based approaches are commonly applied
in the context of autonomous vehicles and delivery robots.

Figure 1.2: The figure illustrates the Thorvald II [1] robot’s capability to operate
in various rough uneven terrains. To fully utilize the robot’s strengths in these
terrains, it is crucial to have a reliable navigation strategy.

However, in more extreme environments, which are characterized by

significant elevation changes, including ramps and bumps, this assumption
falls short. In such settings, incorporating elevation into the environmental
representation becomes crucial during the planning and control phases of
autonomous navigation.
A noteworthy example is a robot operating in a dense forest or engaging in a
search and rescue mission in a rubble-strewn area. In these situations, the robot
faces the nontrivial task of not only determining which regions are traversable
but also devising an optimal motion plan to effectively execute its tasks. This
underscores the importance of a more sophisticated approach to environmental
representation in complex terrain navigation.
The navigation of ground robots in uneven terrains introduces additional
challenges, primarily due to the operational constraints imposed by the terrain’s
surface manifold. These constraints significantly complicate motion planning.
For example, a motion plan designed in full 3D may yield nonviable strategies
in outdoor settings. Consider a scenario where a robot aims to navigate from

3
1. Introduction

one side of a building to the other. A 3D motion plan could illogically propose
a trajectory that involves jumping over the building, which is unfeasible for
ground-based robots.
Therefore, to address these realistic constraints, the majority of the
contributions in this thesis adopt a 2.5D environmental representation. This
approach acknowledges the three-dimensional nature of the terrain while
remaining grounded in the practical limitations of ground robot navigation.
The 2.5D model thus facilitates more feasible and effective motion planning in
complex outdoor environments.
Given the aforementioned challenges, it is imperative to move beyond the
oversimplified assumption that the ground is always effectively 2D. Acknowledging
this, this thesis presents several methodologies that more accurately perceive
the environment as it exists, minimizing assumptions like the world being 2D.
Discarding this 2D assumption introduces complexities in planning and
environmental representation. This transition inevitably leads to increased
demands on time, memory, and implementation, resulting in inefficiencies in
classical planning and navigation algorithms. These challenges necessitate the
development of more sophisticated and efficient approaches to ensure effective
navigation in more complex, three-dimensional terrains.

1.3 Research Questions

The core of this thesis is formed by a series of papers that collectively seek to
address several pivotal research questions (RQs), each aiming to advance the field
of outdoor terrain navigation for autonomous robots. The research questions we
are interested in this thesis are presented below:
RQ1: Current motion planning approaches for navigating uneven terrains
demonstrate varying effectiveness. However, there appears to be a gap in
understanding the full potential of these approaches, particularly in terms
of their adaptability and efficiency for uneven terrains. The literature
also seems to lack a comprehensive exploration of the interplay between
sampling-based motion planning and environmental representation. Given
these circumstances, how can existing motion planning methods for uneven
terrains be enhanced for greater efficacy and adaptability? Furthermore, to what
extent can a synergistic relationship be established between motion planning
and environmental representation, where the environmental model not only
complements but also optimizes motion planning efforts without compromising
the authenticity and fidelity of the real-world environment representation?
RQ2: The inherent inefficiencies in current kinodynamic motion planning
approaches, highlighted by their struggle with high complexity and extensive
state and control dimensions, reveal a significant limitation in the existing
literature. These challenges become even more pronounced when considering the
constraints of outdoor robots, such as limits on velocity, acceleration, or torque.
Given this context, what innovative paradigms could be developed to overcome
these limitations and enable smoother, more optimal trajectory planning for

4
 Contributions

outdoor and all robots under such dynamic constraints? Specifically, how can
these new approaches address the unique constraints of velocity, acceleration, and
torque to enhance the performance and efficiency of outdoor robotic systems?
RQ3: Current approaches to identifying traversable regions in uneven
outdoor terrains predominantly rely on general terrain analysis, focusing on
geometrical and visual characteristics. This approach often overlooks pivotal
factors such as the robot’s heading direction and the varied traversability
capabilities of different robotic systems, revealing a notable gap in the
literature and a shortcoming in traditional offline computation methods. Given
these limitations, is it possible to refine traversability estimation with more
comprehensive and nuanced methods? Specifically, how can these advanced
methods be designed to incorporate often-neglected elements, such as the robot’s
directional orientation and the distinct traversability profiles of various robotic
platforms, thereby enhancing the accuracy and practical relevance of terrain
analysis in robotic navigation?
The answers to these research questions are elaborated upon in a series of
conference and journal papers, the details of which will be comprehensively
provided in the subsequent sections of this thesis. These papers present our
findings and methodologies and contribute significantly to the broader discourse
in the field of autonomous outdoor terrain navigation.

1.4 Contributions
This thesis comprises six comprehensive papers, each constituting a self-contained
contribution to uneven outdoor terrain navigation. While the introduction section
of each paper explicitly outlines its contributions, in this segment of the thesis,
we aim to contextualize these contributions precisely. Our focus here is to
summarize the most notable novel contributions brought by the papers to the
field. The bullet points emphasizing the novel contribution are as follows:

• We developed a novel statistical methodology to evaluate the performance

of sampling-based motion planners, specifically in uneven outdoor terrain
navigation. This approach provides a comprehensive metric for assessing
planner efficiency and robustness.

• Our research has demonstrably shown that integrating environmental

representation with sampling-based planners can significantly enhance
motion planning efficiency—up to fivefold. This improvement is notable
in scenarios involving various robot types, e.g., Ackerman steering or
differential drive types.

• We pioneered an approach wherein reliance on raw point cloud representa-

tions of the environment can effectively bypass conventional sampling-based
planners. By employing connected super voxels as an intermediate repre-
sentation and classical control methods like Model Predictive Control, we

5
1. Introduction

achieved significantly superior motion planning and navigation performance

in real-world scenarios using actual robots.

• To account for the motion planning of dynamic systems, we introduced a

novel heuristic-like function that optimally balances rapid initial exploration
and subsequent solution refinement in kinodynamic motion planning. This
function is part of our multi-threaded, bi-directional tree method, which
significantly outperforms existing kinodynamic planners in efficiency. We
have successfully integrated this planner into the Open Motion Planning
Library (OMPL), a widely used library, enhancing its usability and practical
application.

• We introduced the first neural network designed to process raw LIDAR and
IMU data for real-time, direction-aware traversability assessments using
spatiotemporal point cloud maps, negating the need for real-world data.
Additionally, we developed an efficient simulation pipeline for autonomous
data generation and labeling, leveraging robot experiences across varied
terrains to enhance neural network training with minimal human input.

• In addressing the challenge of integrating complex navigation algorithms

into a unified framework suitable for uneven terrain navigation, we
developed ’vox_nav’1 . This comprehensive navigation framework has been
rigorously tested across various uneven outdoor terrains using two real
robotic platforms. ’vox_nav’ represents a complete navigation solution for
outdoor robotics, incorporating the latest advancements in robot software,
including ROS 2.

1.5 The Papers and Thesis Outline

To facilitate clarity and ease of reference, the papers are systematically
enumerated with numerical letters, ranging from I to VI. This enumeration
not only aids in organizing the content but also provides a coherent structure
for discussing how each paper interconnects and contributes to the overarching
themes and objectives of the thesis.
In Figure 1.3, a detailed illustration is provided that enumerates the papers,
correlates them with the relevant research questions, and highlights their primary
focus areas. The contributions of these studies can be broadly categorized into
four main topics: advanced motion planning strategies, efficient environmental
representation techniques, accurate estimations of terrain traversability, and the
practical application of navigation software in real-world outdoor terrains.
Now, we will delve into the specifics of the contributions, contextualizing
them within the framework of the four identified categories.
Motion Planning: Our research has pinpointed the most effective
sampling-based planners for outdoor navigation, as explored in Paper I. Further
advancements are detailed in Papers II and III, where we introduce novel
1 https://github.com/NMBURobotics/vox_nav

6
 The Papers and Thesis Outline

Figure 1.3: The figure provides a systematic enumeration of the papers, detailing
the specific research questions they address, and outlining the precise context
and focus of each paper.

methodologies that leverage terrain characteristics to enhance the efficiency

of these planners significantly. Additionally, we have critically examined the
limitations inherent in kinodynamic planners, a specialized type of motion
planning designed for systems constrained by kinematic and dynamic factors.
In Paper IV, we introduce our abstractly implemented kinodynamic algorithm,
’CostTrust’, which represents a significant advancement in the field. This
innovation has pushed the efficiency boundaries of current state-of-the-art
kinodynamic algorithms, demonstrating notable improvements in performance
and reliability.
Environmental Representation: This thesis argues that point clouds
offer an exceptionally realistic representation of the world, making them an
effective tool for environmental modeling. However, a challenge arises as most
algorithms require volumetric information about the environment for interaction.
In Papers II and III, we propose efficient algorithms that utilize intermediate
point cloud representations, such as surfels and supervoxels. These algorithms
not only support sampling-based planners but also facilitate classical graph-
based planning algorithms. Moreover, they retain the capability to represent
any scene comprehensively, which is a significant advantage over other popular
methods like Digital Elevation Maps (DEMs). Unlike DEMs, which struggle
with overlapping objects such as bridges or areas under a canopy of trees,
The methodologies introduced in Papers II and III present a more versatile
and accurate approach to environmental representation. These approaches are
designed to work synergistically with planners, enhancing their effectiveness and
efficiency. This synergy is a pivotal aspect of our contribution, demonstrating

7
1. Introduction

the potential of integrated planning and environmental modeling in the context

of autonomous navigation.
Traversability Estimation: Traversability estimation is a crucial element in
the navigation of uneven outdoor terrains. The task of determining traversability
values in such environments is complex, as these values vary depending on the
physical capabilities of different robots and even the direction of the robot’s
movement. In Paper III, we introduced a geometry-only-based method for
traversability estimation. While this approach is efficient and generally effective,
it does not consider the direction the robot is facing, which is a significant
limitation. To address this, our research in Paper V introduces an innovative
approach, employing the first deep neural network that integrates a robot’s
local point cloud map, Inertial Measurement Unit (IMU) data, and locomotion
characteristics to learn terrain traversability based on the robot’s experiences.
We have experimentally validated the superiority of this method’s accuracy over
several established baselines, marking a significant advancement in the field of
autonomous navigation.
Practicality: A key focus of this thesis is the real-world applicability of
both the newly proposed algorithms and existing ones. Integrating complex
navigation algorithms into a unified software framework tailored for uneven
terrain navigation presents significant challenges. While such frameworks exist
for 2D navigation robots [5], the development of a common software solution
for robots operating in uneven outdoor terrains has been largely unexplored. In
Paper IV, we introduce ’vox_nav’, a comprehensive navigation framework. This
framework has been successfully tested in a variety of outdoor uneven terrains
using two real robotic platforms, demonstrating its effectiveness and versatility
in practical scenarios.
This thesis is structured into five distinct chapters: Introduction, Background,
The Papers, Discussion, and a combined section on Conclusion and Future Work.
The Background chapter aims to present essential literature and state-of-the-art
research, establishing a foundational understanding necessary for the subsequent
papers. In the Discussion chapter, we engage in a critical evaluation of the
results yielded by these papers, while also deliberating on the limitations of
the methods employed. The thesis ends with the Conclusion chapter, where we
articulate final reflections and propose potential directions for future research
for uneven outdoor terrain navigation.

8
Chapter 2

Background
In this chapter, we aim to lay the foundational background and effectively
contextualize the scope of this thesis. It is essential to clarify that this chapter
is not intended to be a comprehensive review. Instead, it serves as a concise
overview of the literature and a refresher on key fields pertinent to our research.
Central among these areas are motion planning, environmental representations,
and traversability estimation, all of which are integral to the focus of this thesis.
By providing this foundational knowledge, we aim to equip readers with the
necessary context and understanding required to engage fully with the detailed
discussions and novel contributions that follow in the subsequent chapters of
this thesis.

2.1 Motion Planning

Figure 2.1: The figure illustrates the working principles of the Probabilistic
RoadMap (PRM) [2], a widely recognized sampling-based motion planning
approach.

9
2. Background

In the field of robotics, motion planning involves formulating a sequence of

contiguous path segments that enable a robot to transition from a start state,
denoted as XS , to a goal state, XG , while concurrently avoiding environmental
collisions. When dealing with robots subject to kinematic or dynamic constraints,
additional complexities arise in the planning process. The chosen strategy
for motion planning is heavily influenced by the method of environmental
representation, as this determines the assessment of collision states.
A prevalent approach in motion planning employs grid cell-based methods.
Here, grid cells are conceptualized as nodes within a graph structure, intercon-
nected to form a navigable network. Utilizing this structure, classical graph
search algorithms like Breadth-First Search (BFS), Depth-First Search (DFS),
Dijkstra’s algorithm, or A* are employed to identify a viable path from the start
node to the goal node. This approach, while common, varies in its application
depending on the specific environmental and robotic constraints at play.
Beyond grid cell-based algorithms, motion planning in robotics also leverages
spline-based and Bézier curve-based methods for generating smooth trajectories.
Additionally, for robots operating under specific curve constraints, algorithms like
Dubins[6] paths or Reeds-Shepp[7] curves, which focus on minimal connecting
curves, are frequently employed. Dynamic environments often necessitate the
use of control-based approaches such as the Dynamic Window Approach (DWA)
[8] or Model Predictive Control (MPC).
These methods have been predominantly successful in scenarios involving
robots navigating primarily flat, 2D environments. The efficacy of these
approaches in such contexts is well-documented, highlighting their suitability for
environments with relatively flat topography.
In this thesis, we place significant emphasis on sampling-based approaches,
which have gained popularity in recent times due to their demonstrated efficiency
in scenarios involving large state dimensions. These approaches interact with
environmental representation in a more abstract manner, which makes them
particularly suited for motion planning in expansive environments. Notably,
they do not necessitate a prior model of environmental representation, either
discrete or continuous, such as a 2D or 3D occupancy voxel grid.
The diagram in Figure 2.1 provides an instructive representation of the
Probabilistic RoadMap (PRM), a prevalent sampling-based planner. In this
method, a ’sample’ state is uniformly selected from the state space and checked
for its collision-free status, depicted in blue. Samples in collision, marked in
red, are excluded. Collision-free samples are added to a ’RoadMap,’ where
neighboring nodes within a certain proximity are connected, forming a graph.
The algorithm persistently evaluates the feasibility of creating a path from the
start to the goal within this graph. While other sampling-based planners follow
analogous principles, their sample connection strategies may differ, with some
utilizing tree-based approaches.
Similarly, Figure 2.2 illustrates the workings of the Rapidly-exploring Random
Tree (RRT). This algorithm distinctively employs a tree structure, with the root
being the start node. The tree then rapidly expands through the collision-free
state space, aiming to forge a path towards the goal node. This growth is

10
 Motion Planning

Figure 2.2: This figure illustrates the Rapidly-Exploring Random Tree (RRT)
algorithm, which employs a tree structure to systematically search the state
space and establish a path to the desired goal state. The inherent structure of
RRT, based on tree construction, renders it less suitable for re-planning scenarios,
as a new tree must be generated for each new set of start and goal pairs.

characterized by a directed exploration, where new samples are incrementally

added, extending the tree towards unexplored areas and potentially towards the
goal.
A key requirement for these methods is the implementation of a collision
check module. This function is essential as it determines whether a specific
state resides in a collision path. The flexibility and efficiency of sampling-based
approaches, combined with the strategic use of collision check modules, allow for
effective motion planning in complex and large-scale environments.
Among the foremost sampling-based algorithms in this domain are the
Probabilistic RoadMap (PRM) algorithm [2] and the Rapidly-exploring Random
Tree (RRT) algorithm [9], each with a number of variants and improved versions.
The seminal work by Karaman and Frazzoli [10] provides a thorough analysis of
the optimality, completeness, and guarantees of sampling-based planners. The
development of libraries and frameworks like the Open Motion Planning Library
(OMPL)[11] has been instrumental in facilitating the use and implementation of
these algorithms. OMPL includes a wide array of sampling-based planners such
as RRT* and PRM*[10], and more recent algorithms that integrate graph and

11
2. Background

tree structures, like BIT*[12], ABIT*[13], and AIT*[14]. Planners utilizing

parallelized strategies, such as CFOREST[15] and AnytimePartShortening
(APS) [16], have improved performance through the concurrent execution of
multiple planning threads.
The extension of sampling-based planners to kinodynamic planning (see
Figure 2.3), which includes considerations of dynamics such as velocity and
acceleration, has led to variants of the RRT algorithm, notably RRT-Extend [17]
and Expansive Space Trees (EST)[18]. These focus on dynamic forward
propagation and efficient state space exploration. Building upon the foundations
laid by Karaman and Frazzoli[10], subsequent research has achieved asymptotic
optimality in kinodynamic planning, often depending on a steering function to
compute optimal paths in obstacle-free scenarios. This function typically solves
a two-point boundary value problem (BVP), a complex task for intricate systems
such as a quadrotor with 15 degrees of freedom.

Figure 2.3: This figure depicts kinodynamic motion planning using a modified
RRT algorithm. Each new sample must conform to the system’s motion
constraints in this process, often achieved through steering functions or
random forward propagation with varied control inputs. While this restricts
maneuverability, it’s crucial for ensuring smooth and feasible motion plans. In the
realm of kinodynamic planning, the paramount concern is the strict adherence
to both physical constraints and control input limitations.

Recent endeavors in the motion planning community have focused on reducing

12
 Motion Planning

the dependence on steering functions. Li et al.[19] introduced the SST algorithm,

a kinodynamic planner based on a simplified random forward search tree
independent of steering functions, and demonstrated its asymptotic optimality
given adequate Monte Carlo propagations. Hauser et al.[20] presented a meta-
algorithm that produces an asymptotically optimal kinodynamic planner without
the need for steering functions or numerical BVP solvers. The increasing use
of heuristics in kinodynamic planning, as explored by Littlefield et al.[21] and
furthered by Kleinbort et al.[22, 23], has led to an enhanced understanding of
asymptotically optimal kinodynamic planning with fewer assumptions. Most
recently, Shome et al. [24] proposed a new approach for kinodynamic motion
planning using sampling techniques to estimate connectivity in high-dimensional
configuration spaces, proving to be asymptotically optimal and efficient in finding
high-quality solutions.

This thesis addresses certain limitations of sampling-based planning ap-

proaches, particularly in the context of robots navigating uneven terrains. A
key limitation identified is the strategy used in sampling. Given our focus on
robots operating on uneven terrains, these robots are inherently limited to the
surface manifold of the environment. Our research has led to the development
of innovative strategies that significantly enhance the efficiency of sampling
methodologies, thereby improving the overall performance of sampling-based
planners.

Furthermore, we have introduced the concept of an ’elevation state-space,’

which resembles a 2.5D-like state space. This state-space, unlike a Digital
Elevation Map (DEM), is capable of representing overlapping structures, such
as bridges. The adoption of this elevation state-space has allowed major
sampling-based planners to be effectively applied in the context of uneven
terrain navigation, eliminating the need for additional feasibility checks. This
advancement not only broadens the applicability of these planners but also
enhances their effectiveness in complex, multi-level environments.

In addition to the aforementioned contrasts that delineate our research from

existing methodologies, our work in kinodynamic motion planning specifically
addresses the challenges associated with the efficiency limitations of current
approaches. Kinodynamic planning necessitates the exploration of both system
states and control inputs, with the latter required to conform to specified
limitations. Our approach innovatively incorporates heuristics to guide both
the exploration and exploitation phases of the planning process. This heuristic-
driven method has demonstrated marked improvements in efficiency compared
to state-of-the-art kinodynamic planners. By optimizing the exploration strategy,
our research not only adheres to the system’s dynamic constraints but also
enhances the overall planning efficiency, offering a significant advancement in
the field of kinodynamic motion planning.

13
2. Background

2.2 Environmental Representation

In uneven terrain navigation, environmental representation facilitates the
planners and controllers to ensure collision-free movement of the robot.
Environmental representation is crucial as it can affect the performance of
the planners and controller and the overall navigation pipeline. In addition to
performance, there are also memory considerations, as beyond 2D spaces, the
memory consumption increases dramatically for refined resolution volumetric
representations.
The common environmental representations, which also can be mentioned as
"maps" in some other literature, depend on the type of environment. For uneven
terrain navigation, 2D occupancy grid cells, DEM, Octomap, VDB-Grids, and
Meshes have been used.

Figure 2.4: Illustrated are various environmental representations for uneven

terrain navigation: a. 2D occupancy grid, b. DEM, c. 3D OctoMap, d.
3D Mesh, e. VDB-grid, and f. raw point clouds. This thesis highlights the
importance of selecting an appropriate environmental representation based
on the robot’s operational environment and explores the integration of these
representations with motion planning and control. The focus is on enhancing
motion planning effectiveness through raw point cloud-based methods, thereby
improving navigation plans for robots in complex terrains.

In Figure 2.4, we illustrate various popular methods of environmental

representation. These methods have been effectively employed in diverse
scenarios, including outdoor robot navigation, robotic manipulation, and
exploration. Within the scope of this thesis, our objective is to further exploit
these environmental representations. This endeavor aims to optimize motion
planners and controllers, thereby refining the navigation capabilities of robots
traversing uneven outdoor terrains.

14
 Environmental Representation

The Navigation2 framework [25], along with its predecessor, the original ROS
navigation framework, is known for its widespread adoption in both academic
and industrial contexts [5]. These frameworks predominantly rely on a 2D
occupancy grid for environmental representation, which has proven practical
and widely utilized in various research projects. However, their reliance on 2D
occupancy grids limits their applicability in navigating uneven terrains.
Digital Elevation Maps (DEMs) represent an alternative approach to
environmental mapping. A DEM is essentially a grid map where each grid cell
contains elevation data for specific x and y coordinates, expressed as a function
z = f (x, y). Wermelinger et al. [26] expanded on this concept by incorporating
a traversability measure within each cell of the DEM. This adaptation provides
elevation information and evaluates the terrain’s navigability, enhancing the
utility of DEMs in complex environments. However, DEMs have limitations
in accurately capturing environments with layered structures. They fall short
in scenarios involving underpasses, such as beneath bridges, under dense tree
canopies, or overhanging structures, which are common in outdoor terrains.
This shortcoming is due to the inherent nature of DEMs, which typically
represent terrain as a single-layered surface, thus failing to account for the
vertical complexity found in such layered environments.
An advanced OcTree-based 3D occupancy method, OctoMap, was developed
by Hornung et al.[27]. OctoMap distinguishes itself by utilizing a probabilistic
approach to determine the occupancy values of geometric voxels. This method
is specifically designed to account for sensor noise and uncertainties prevalent
in real-world scenarios. Building on OctoMap, Wang et al.[28] implemented
a framework that leveraged OctoMap for navigating indoor mobile robots in
uneven environments. Their approach showcases the adaptability of OctoMap in
complex, real-world applications beyond its initial intended use cases. However,
the suitability of OctoMap for navigating uneven terrains raises certain concerns.
A key issue is its tendency to retain numerous cells that may not be relevant
to the navigation task. This retention leads to a suboptimal memory footprint,
unnecessarily increasing data storage. Furthermore, this excess can result
in heightened computational costs during collision-checking processes. Such
inefficiencies are particularly problematic in complex terrains, where optimal
memory usage and efficient collision detection are crucial for effective navigation.
As discussed in [29, 30], other methodologies have employed 3D meshes
derived from point cloud maps to represent uneven terrains. These approaches
leverage the edge connectivity of 3D meshes to devise navigation strategies
suitable for outdoor environments with complex structures. For instance, the
authors in [29] utilized a 3D mesh to construct the edge connectivity of such
environments. Subsequently, graph-based search algorithms are applied to
this connectivity framework to generate feasible navigation paths. Ruetz et
al. [30] also explored a 3D mesh-based framework in their research. While mesh
connectivity methods are designed to aim for robot independence, they primarily
focus on the geometrical aspects of environmental representation. This approach,
however, does not explicitly accommodate the kinematic and dynamic constraints
inherent to specific types of robots, particularly those equipped with Ackerman

15
2. Background

steering systems. Therefore, the purely geometrical nature of these methods may
limit their applicability in scenarios where such constraints play a critical role in
the robot’s navigation and maneuverability capabilities. The primary distinction
of this thesis’s contribution to environmental representation, particularly for
optimized navigation across uneven terrains, is its consideration of kinematic
and dynamic constraints. This focus sets it apart from previously proposed
mesh-based representations, which typically do not account for these crucial
factors as the mesh-based motion plans are geometric but not kinodynamic.
Previous methods in the field have often lacked a focused emphasis on the
interplay between environmental representation and motion planning, leading to
incompatible and redundant implementations of path-planning algorithms across
different environmental representations. In stark contrast, this thesis significantly
emphasizes the synergy between environmental representation and planning. We
argue that integrating these two components is essential for effective planning
and navigation in challenging uneven terrains.

2.3 Traversablity Estimation

The concept of traversability estimation in the realm of uneven terrain navigation
goes beyond the simplistic binary classification of regions as merely traversable
or non-traversable. The essence of its significance lies in the ability to ascertain
a continuous measure of traversability. Such a nuanced assessment enables a
robot navigating through uneven terrains to identify areas with lesser roughness
or incline, thereby augmenting both the safety and efficiency of its navigational
processes.
In this domain, the primary function of traversability estimation is to assign
specific traversability or cost values to localized segments of the environment. As
delineated in the previous section of this thesis, environmental representations are
typically rendered at a certain resolution, essentially discretizing the environment
into ’cells.’ For example, in a 2D occupancy grid, each cell may be assigned a
cost value reflective of the likelihood that it falls within a traversable or non-
traversable area. This approach offers a more granular and practical means of
assessing the traversability of different terrain types, thereby facilitating more
informed and strategic navigation decisions by the robot.
Traversability estimation has evolved significantly over time. Early efforts
primarily relied on vision-based techniques, with stereo vision playing a
pivotal role in understanding a scene’s depth. Early works in this field, like
Manduchi [31] and Bajracharya [32], utilized stereo-range data to classify surfaces,
acknowledging challenges like varying illumination. Before the widespread
adoption of deep learning, traditional machine learning methods were prevalent.
Angelova et al.[33] introduced a decision tree classifier, and Bajracharya et al.[34]
presented a hybrid SVM-Bayesian classifier. These methods, often limited by
their reliance on hand-crafted features, paved the way for more sophisticated
techniques.
Kelly’s [35] comparison between SVMs and CNNs for terrain traversability

16
 Traversablity Estimation

Figure 2.5: Diverse traversability estimation methods for varied environments are
depicted. (a) Bayesian Generalized Kernel-based traversability estimates applied
to Digital Elevation Maps (DEM)[3], (b) image-based terrain classification[4],
(c) obstacle proximity-based 2D costmaps[5], and (d) our contribution of a deep
neural network-based traversability estimation on 3D point clouds.

classification marked a shift towards deep learning, promising more nuanced

traversability insights into complex terrains. Recent advancements by Leung
et al.[36] and Hosseinpoor et al.[37] have further leveraged deep learning for
sophisticated terrain traversability analysis.
In the realm of LIDAR-based approaches, notable contributions include
Agishev et al.’s [38] point cloud geometry technique and Waibel et al.’s [39]
combination of LIDAR and inertial sensing. These methods offer detailed
traversablity analysis, though sometimes lacking in aspects like directionality
awareness. Martinez et al.[40] and Thakker et al.[41] have also made significant
contributions to this field.
Hybrid methods that combine RGB imaging with LIDAR data are increasingly
popular. Examples include Wallin et al.’s [42] high-resolution topography
approach and Fusaro et al.’s [43] SVM classifier. Zhang et al.[44] and Haddeler et
al.[45] have expanded terrain traversability analysis beyond urban environments,
addressing more complex uneven landscapes.
Guan et al.’s [46] method integrates LIDAR with RGB for excavator
navigation, and Borges et al.’s [47] comprehensive survey offers a deep dive

17
2. Background

into various stata-of-the-art methodologies.

Recent developments in the field of traversability estimation have shown a
marked preference for deep learning methodologies over traditional approaches.
As highlighted by Kelly [35], traditional techniques, while often requiring less
data, encounter difficulties in terrains that lack distinctive features. Addressing
this challenge, as part of this thesis, our research introduces a novel deep neural
network architecture specifically designed for computing continuous traversability
values across a diverse range of terrains. To circumvent the inherent data
inefficiency of deep learning, the method employs an automated data generation
method leveraging high-fidelity simulation environments. A distinctive feature
of our approach is the integration of inertial sensing with spatiotemporal LIDAR
point clouds, facilitating the generation of comprehensive point cloud maps.
These maps are instrumental in deriving accurate traversability estimates around
the robot, thereby significantly enhancing the efficacy of local motion planning
and control and contributing to safer navigation in complex terrains.

2.4 Emerging Approaches in Uneven Terrain Navigation

In the realm of uneven terrain navigation, it is crucial to adopt a holistic
perspective that encompasses various aspects of the field. While many studies
concentrate on specific components like motion planning, there is a growing body
of research that addresses uneven terrain navigation in a more integrated manner.
Notable among these is the integration of sampling-based motion planners with
terrain data, as demonstrated in studies like [48] and [49], which facilitate the
generation of feasible motion plans subsequently integrated with a trajectory
tracking controller.
Another emerging trend is the incorporation of uncertainties into the planning
and control phases, as explored in works like [50] and [51]. These methodologies
often feed their plans into controllers, typically Dynamic Window Approach
(DWA) [8] or Model Predictive Control (MPC), which can dynamically evade
obstacles detected through enhanced perception capabilities.
Additionally, there has been a surge in end-to-end learning-based approaches
for navigating uneven terrains. These methods typically employ camera or
LIDAR inputs to directly output optimal controls for goal-oriented or exploratory
navigation. Despite the apparent simplification offered by end-to-end methods
compared to traditional modular approaches, current literature indicates that
they often replicate existing classical results [52]. A major challenge with end-to-
end learning is ensuring safety guarantees, a feature more readily provided by
methods like MPC that utilize bounded uncertainty models. Furthermore, the
explainability of end-to-end approaches remains a significant area of research,
not only in robotic navigation but across the broader field of deep learning [53].
Given the current trajectory of research in learning-based approaches, it is
conceivable that end-to-end methods might surpass traditional block-structured
navigation approaches in terms of performance in the near future. This potential

18
 Emerging Approaches in Uneven Terrain Navigation

shift underscores the dynamic nature of research in uneven terrain navigation

and the importance of continual exploration and adaptation in the field.

19
Chapter 3

Methodological Framework
3.1 Explanation of Research Design
This thesis employs a hybrid methodology, combining both quantitative and
qualitative research methods, to analyze the obtained results effectively. A
significant portion of the problems addressed in this thesis are quantifiable. For
instance, in our aim to optimize the efficiency and quality of motion plans across
uneven terrains, we evaluated various path characteristics, including path length,
curvature, and accumulative control duration.
However, there are scenarios where quantitative evaluation alone is insufficient.
A notable example is the estimation of traversability for uneven terrains.
Currently, there is no established dataset or benchmark for non-binary
traversability estimation, making quantitative comparisons with existing methods
challenging. This difficulty is compounded by the influence of a robot’s
locomotion capabilities on the results. To address this limitation, this thesis also
incorporates qualitative evaluations in the contributed papers. We conducted
numerous field experiments, both in simulations and in real-world scenarios, to
assess the effectiveness of our approaches in practical settings.

3.2 Robot Platforms and Data

This thesis places significant emphasis on the practical validation of our research
findings using real robots and environments. Our experiments primarily utilized
two real robotic platforms, each outfitted with commonly employed sensors such
as LIDARs, IMUs, and GNSS systems. The primary platform for our research
was the Thorvald II [1], a versatile robot adept at navigating uneven terrains
and featuring modular, lego-like construction. In addition, we employed a more
compact robot from Robotnik, as depicted on the right side of Figure 3.1. This
variety in platforms allowed for a comprehensive assessment of our methodologies
across different robotic configurations and operational contexts.
The sensor suite employed in this thesis comprises LIDAR, wheel odometers,
Inertial Measurement Units (IMUs), and Global Navigation Satellite System
(GNSS) devices. The specifics of this suite, including its integration and
functionality on our robotic platform, are detailed in Figure 3.2.
The selection of LIDAR as a primary sensor in this thesis is predicated on
its high-precision measurement capabilities and robustness in varying outdoor
lighting conditions. Furthermore, LIDAR offers the significant advantage of
generating highly dense and accurate point cloud maps. These maps are
instrumental in facilitating advanced localization techniques through the use of
recent Simultaneous Localization and Mapping (SLAM) approaches, thereby

21
3. Methodological Framework

Figure 3.1: The figure displays two robotic platforms: the Thorvald II robot
on the left and the Robotnik platform on the right. Both platforms have a
consistent sensor suite comprising LIDAR, Inertial Measurement Units (IMUs),
wheel encoders, and GNSS receivers.

contributing significantly to the effectiveness of our robotic navigation systems

in complex environments.

Figure 3.2: Sensors mounted on a robot are depicted on the left. The robot’s pose
is determined by combining multiple sensory data using an Extended Kalman
Filter (EKF). The dark red points are from the live LIDAR sensor that precisely
overlaps the point cloud map, indicating precise localization.

In Figure 3.2, we present the configuration of our robotic platform and

22
 Environments and Simulations

the methods employed for its localization. The integration of GNSS data into
our localization framework is dependent on weather conditions. Primarily,
localization is achieved through the fusion of data from the Inertial Measurement
Unit (IMU), LIDAR odometry, and wheel odometry using an Extended Kalman
Filter (EKF). The inclusion of GNSS data is optional and is utilized based on
its feasibility under varying weather scenarios.

3.3 Environments and Simulations

The primary site for data collection and experimental trials in this research was
the Norwegian University of Life Sciences (NMBU) in Ås City, Norway. The
diverse environments at NMBU encompass a range of elements such as varying
elevations, buildings, trees, and dynamic entities like pedestrians. This variety
provided a comprehensive setting for testing and evaluating the capabilities of
our robotic platforms under different real-world conditions.

Figure 3.3: This figure presents a collection of satellite imagery and first-person
photographs of the testing sites.

The experiments conducted as part of this research were carried out across
various seasons, encompassing a range of weather conditions. To visually
represent the testing environments, Figure 3.3 includes satellite imagery from
Google Maps and first-person view photographs.
In the course of this thesis, we extensively utilized simulation tools for the
development and testing of our algorithms. The primary simulators employed
were the Gazebo simulator [54] and a Unity game engine-based simulator. The
Gazebo simulator is particularly beneficial due to its seamless integration with
ROS [55] and its capability to simulate various sensor plugins, which facilitates
an efficient transition from simulation to real-world application. However, when
high-fidelity simulation was required, we found that the Unity game engine-based

23
3. Methodological Framework

simulator offered several distinct advantages, particularly in terms of visual

realism and the ability to simulate complex environmental interactions.

3.4 Application Scenarios

Outdoor environments, often characterized by their uneven terrains, present
unique challenges for navigation and motion planning. The methodologies
developed through this thesis are versatile and can be applied to various uneven
terrains. Here, we highlight specific application scenarios where our research
could have a significant impact.

3.4.1 Precision Agriculture on Hilly Farms

In the agricultural sector, mid-sized robots like the Thorvald II [1] or autonomous
tractors equipped with GNNS and advanced navigation systems are ideal for
precision farming in hilly and uneven terrains. These robotic systems can
enhance the efficiency of planting, fertilizing, and harvesting processes by
dynamically adapting their navigation and operational strategies to the contours
and conditions of the terrain.

3.4.2 Dense Forest Exploration

Consider a scenario where a ground robot is assigned to explore and map a
dense, previously uncharted forest. This environment poses several challenges,
including the potential unavailability of GNSS signals due to the dense tree canopy
overhead. Additional hurdles include the need for robust traversability estimation
and effective motion planning. The autonomy enabled in such environments by
our research can offer substantial benefits, including cost reduction and a deeper
understanding of the forest’s structure and composition.

3.4.3 Delivery Robots in Scarcely Populated Towns

Delivery robots are commonly operational in well-structured, flat urban
environments with designated areas for pedestrian movement. However, in
smaller towns with less population density, the road infrastructure may not be as
organized or predictable as in larger cities. In such settings, delivery robots might
need to traverse footpaths or climb sidewalks to reach their destinations. These
atypical and uneven surfaces present unique challenges that standard urban
navigation methods cannot adequately address. Therefore, in these scenarios,
the application of navigation methodologies developed for uneven terrain, as
explored in this thesis, becomes crucial. These methods might enable delivery
robots to adapt to less structured environments, ensuring efficient and reliable
service in a broader range of urban and semi-urban settings.

24
 Software Tools

3.5 Software Tools

The experimental work and implementation of our algorithms within this thesis
were primarily conducted using the ROS 2 framework. ROS 2 is an evolved
version of the original Robot Operating System (ROS) and utilizes a publish-
subscribe paradigm for messaging and data exchange among various concurrently
running programs. For instance, one program might analyze traversability while
another performs localization, both potentially utilizing a common sensory output
like LIDAR point clouds. In ROS terminology, these programs are referred to as
’nodes.’ As a practical contribution of this thesis, we have developed and provided
a complete navigation framework for uneven terrains, named ’vox_nav.’ This
framework integrates various functionalities and algorithms, demonstrating their
applicability in real-world uneven terrain navigation scenarios. In addition to
the ROS 2 framework, our research also leveraged several other notable software
libraries. The Open Motion Planning Library (OMPL) was instrumental for the
development and benchmarking of motion planning algorithms against state-of-
the-art standards. For the implementation of Model Predictive Control (MPC),
we employed CasAdi [56] and Acado [56], which are specialized in optimization
and control. Furthermore, the robot_localization package was utilized for
Extended Kalman Filter (EKF) implementations. Each of these libraries played
a crucial role in enhancing the functionality and efficiency of our algorithms and
contributed significantly to the overall success of our experimental endeavors.

3.6 Limitations
Despite the significant contributions of this thesis to the field of uneven terrain
navigation, it is important to acknowledge certain limitations. The most notable
of these is the lack of consideration for robot-terrain interactions. Ideally, this
aspect could be addressed through the estimation of soil parameters using a
range of sensors, including RGB cameras. However, our primary reliance on
LIDAR, which provides solely geometric measurements, precluded the reliable
prediction of soil parameters. This limitation is particularly relevant in scenarios
involving soft soil, where the absence of soil characteristic data could increase
the risk of the robot becoming stuck.
Another limitation in our research pertains to the initial reliance on pre-built
maps. Although the necessity of moving beyond the constraint of pre-existing
maps became apparent, this realization did not emerge until the later stages of
our research. The initial dependence on pre-built maps potentially restricted the
versatility and adaptability of our navigation algorithms in dynamically changing
or previously unmapped environments.
The absence of additional sensors, such as RGB cameras, in our research
constitutes another limitation. The integration of such sensors could have
significantly enhanced the estimation of soil parameters and the accuracy of
traversability assessments, especially in challenging environments like tall grass
areas. While the incorporation of these sensors would have potentially improved

25
3. Methodological Framework

the overall performance of the navigation system, it is important to acknowledge

that this would also have increased the system’s complexity. Balancing
performance enhancement with system complexity is a critical consideration in
the design of autonomous navigation systems and represents an area for potential
refinement in future research.
Taking notes on these limitations in future work could substantially enhance
the robot’s ability to navigate safely and effectively in a wider range of terrains.

26
Chapter 4

Paper Summaries

4.1 Paper I: Benchmark of Sampling-Based Optimizing

Planners for Outdoor Robot Navigation

Paper I, titled ’Benchmark of Sampling-Based Optimizing Planners for Outdoor

Robot Navigation,’ conducts a critical evaluation of recent state-of-the-art
Sampling-Based Optimizing (SBO) motion planners from the Open Motion
Planning Library (OMPL). This paper focuses on assessing these planners’
effectiveness in the context of outdoor robot navigation, particularly on uneven
terrains. The aim is to identify the most proficient state-of-the-art methods and
to pinpoint the common characteristics that set efficient motion planners apart
from the rest. This study has provided us with an understanding of current
trends and ways to improve the performance of motion planners in outdoor
terrains.

The research in this paper is characterized by its meticulous approach,

employing an automated method for generating motion planning problems
specifically designed for outdoor scenarios. Through a comparative analysis
of various planners based on key metrics such as path length, smoothness,
and success rate, the study conducts a thorough assessment of each planner’s
performance. Given the probabilistic nature of sampling-based planners, the
approach adopted in this paper relies on statistical analysis. This analysis is
grounded in a number of experimental samples, ensuring robustness and reliability
in the conclusions drawn. By employing this statistical framework, the paper
effectively navigates the inherent uncertainties associated with probabilistic
planners, providing a more comprehensive and reliable evaluation of their
performance. The findings of this research are especially relevant in the field
of outdoor terrain navigation. They provide crucial insights into selecting and
applying the most effective sampling-based optimization (SBO) planners for
complex outdoor environments. This knowledge contributes to enhancing the
capabilities and efficiency of mobile robots operating in these demanding settings.

This paper makes a contribution to the field of outdoor terrain navigation

by methodically benchmarking and pinpointing planners that exhibit superior
performance for outdoor robotic tasks. It fills a critical gap in the existing
literature by providing a focused performance analysis of sampling-based
optimization (SBO) path planning in the context of outdoor navigation. The
insights and recommendations derived from this study are instrumental in guiding
the selection of appropriate planners for outdoor navigation tasks, thereby
advancing the field by addressing a previously unmet need.

27
4. Paper Summaries

4.2 Paper II: Elevation State-Space: Surfel-Based Navigation

in Uneven Environments for Mobile Robots
Paper II, titled "Elevation State-Space: Surfel-Based Navigation in Uneven
Environments for Mobile Robots," focuses on improving robot navigation
in uneven terrains. It addresses the challenge of efficiently and accurately
navigating complex outdoor environments by introducing a novel surfel-based
representation of point clouds. This method integrates robots’ kinematic and
physical constraints with existing sampling-based motion planning algorithms,
enhancing the performance and success rates of sampling-based planners in
unstructured, uneven terrains by huge margins.
The methodology in "Elevation State-Space: Surfel-Based Navigation in
Uneven Environments for Mobile Robots" revolves around a novel surfel-based
state-space representation. This approach utilizes surfels (surface elements) to
create a more detailed and adaptable representation of uneven terrains from
point cloud data. It integrates this representation with traditional sampling-
based motion planning algorithms such as PRM*, taking into account both the
kinematic and physical constraints of the robot. The integration of surfels during
the ’sampling’ phase of the planning process significantly enhances efficiency
by preemptively eliminating the sampling of non-feasible states. This strategic
approach transforms the motion plan search into a guided process, as opposed to
a blind, uniform sampling method. By filtering out impractical states early, the
planning algorithm is steered toward more viable solutions, thereby streamlining
the search process and potentially improving the overall effectiveness of motion
planning in complex and unstructured outdoor environments. The method’s
efficacy is demonstrated through experiments in both real-world and simulated
scenarios, showcasing its potential to enhance robotic navigation on uneven
terrain.
The novel contributions of this paper include the development of a surfel-
based state-space representation for uneven terrains and its integration with
robot motion planning algorithms within OMPL. This approach addresses the
limitations of traditional navigation methods in complex terrains arising from
the exclusion of surface constraints from planning. Key results show improved
navigation efficiency and success rates (up to 5X) in both simulated and real-
world unstructured environments, demonstrating the practical applicability and
effectiveness of this surfel-based method for robotic navigation on uneven terrain.

4.3 Paper III: Navigating in 3D Uneven Environments

Through Supervoxels and Nonlinear MPC
The third contribution, Paper III, "Navigating in 3D Uneven Environments
through Supervoxels and Nonlinear MPC", is centered on enhancing autonomous
navigation in complex and uneven three-dimensional terrains. It addresses the
challenge of accurately and efficiently navigating such environments in which
traditional navigation methods struggle with efficiency issues, which Paper II

28
 Paper IV: CostTrust: A Fast-Exploring, Iteratively Expanding Frontier-Based
Kinodynamic Motion Planner
improved to an extent. The motivation is to go beyond the improvements of
Paper II. Paper III introduces a novel approach combining supervoxel-based
point cloud representations for terrain mapping and a Nonlinear Model Predictive
Controller (NMPC) for motion planning. This method aims to provide more
precise, adaptable, and efficient navigation in varied and challenging terrains by
bypassing conventional sampling-based motion planners.
The approach used in this paper is distinct from existing works in its
integration of supervoxel-based point cloud representations and Nonlinear Model
Predictive Control (NMPC). Supervoxels represent an efficient method for
clustering point clouds while respecting object boundaries. This characteristic
renders them particularly suitable for uneven terrain navigation. The key
advantage of super voxels lies in their ability to construct a detailed connectivity
mesh over extensive point clouds efficiently. This mesh is a foundational structure
that various graph algorithms can leverage to determine optimal navigation paths.
Using this mesh, planners can effectively create courses from a start supervoxel
to a goal supervoxel, navigating through complex terrains with a higher level of
granularity and precision. The utilization of supervoxels, therefore, significantly
enhances the capability of navigation systems to process and interpret large-
scale spatial data in uneven terrains. NMPC, used for obstacle avoidance and
motion plan tracking, offers kinodynamic feasibility in resulting movements, a
notable advancement over simpler, geometry-based planners. This combination
allows for more accurate, dynamic, and efficient navigation in uneven outdoor
environments, setting it apart from existing methodologies.
The approach was evaluated through extensive simulations and real-world
tests. These evaluations focused on the method’s performance in accurately
and efficiently navigating uneven terrains. The results demonstrated a notable
improvement over existing navigation methods, particularly regarding path
efficiency and execution time. The conclusions underscore the effectiveness
of the combined use of supervoxel-based terrain representation and NMPC for
autonomous navigation in challenging 3D environments, highlighting its potential
to enhance robotic mobility in real-world applications.
The research detailed in this paper garnered attention when presented at
a conference, leading to an invitation from the conference chairs to develop
an extended version for publication in a reputable journal. This invitation
reflects the recognized potential and impact of the research within the academic
community, underscoring its contribution to the field.

4.4 Paper IV: CostTrust: A Fast-Exploring, Iteratively

Expanding Frontier-Based Kinodynamic Motion Planner
Paper IV, "CostTrust: A Fast-Exploring Iteratively Expanding Frontier-Based
Kinodynamic Motion Planner," focuses on improving kinodynamic motion
planning in robotics beyond outdoor ground robots. It introduces a novel,
efficient algorithm that balances exploration and exploitation dynamically. The
need for this research arises from the limitations of existing motion planning

29
4. Paper Summaries

methods in dynamic and high-dimensional environments. Its significance for

uneven terrain navigation lies in its application to robots like Ackermann-steered
robots with control input limits to steering angle or velocity.
The method presented in this paper involves a bi-directional, multi-threaded
algorithm that dynamically balances exploration and exploitation in kinodynamic
motion planning. This method includes a heuristic-like function and a tree
search mechanism. The heuristic-like function aids in efficiently guiding the
search process, making it vastly exploratory, while the bi-directional tree search,
executed in a multi-threaded manner, significantly speeds up the planning process.
These components of the method work in tandem to enhance planning efficiency,
particularly in complex systems with high state and control dimensions.
The efficacy of this method was rigorously assessed through simulations
involving an Ackermann-steered robot, characterized by four states and two
control dimensions, and a dynamic quadrotor with fifteen states and six control
dimensions. This evaluation included benchmarking the performance against
established kinodynamic motion planning algorithms within OMPL. The results
highlighted that the CostTrust algorithm outperformed others in terms of solution
rate percentage and path cost efficiency. Notably, the path cost was calculated
based on the time required for a robot to traverse a path, with shorter paths
yielding more favorable costs.
These findings underscore the effectiveness of the CostTrust algorithm in
kinodynamic motion planning, particularly in dynamic and high-dimensional
environments, such as outdoor terrain navigation involving robots with dynamic
constraints such as limitations on the velocity and acceleration of torque on
motors.

4.5 Paper V: From Simulation to Field: Learning Terrain

Traversability for Real-World Deployment
Paper V, titled ’From Simulation to Field: Learning Terrain Traversability for
Real-World Deployment,’ focuses on enhancing robotic navigation in unstructured
outdoor environments through the prediction of terrain traversability. This
research addresses the critical challenge faced by autonomous systems in
effectively assessing and navigating diverse and uneven terrains. A significant
innovation of this paper is the employment of a deep neural network that
processes data from LIDAR and IMUs. This network is distinctively trained
using the locomotion experiences of a robot within high-fidelity simulations. Such
an approach successfully bridges the often challenging transition from simulated
training environments to real-world applications, marking a considerable leap
forward in the field of uneven terrain navigation. It demonstrates the potential
of advanced machine learning techniques to adapt and generalize from simulated
scenarios to practical, real-world settings.
The methodology presented in Paper V (see Figure 4.1) integrates a deep
neural network with LIDAR and IMU data for the assessment of terrain
traversability. Central to this approach is the neural network’s training process,

30
 Paper VI: Enabling Robot Autonomy through a Modular Software Framework

which utilizes data generated from the robot’s movement across high-fidelity
simulations. This process is supported by a novel pipeline for data generation and
labeling, enhancing the quality and relevance of the training data. Once trained,
the neural network, combined with the sensory input, provides continuous and
reliable assessments of terrain traversability. This synergistic use of advanced
neural network techniques and simulated training data effectively bridges the
gap between simulated environments and the complexities of real-world terrain
navigation, offering a significant advancement in the field.

Figure 4.1: This figure presents the components constructing the approach in
Paper V.

The proposed method was evaluated through extensive testing in both

simulated and real-world environments, providing a thorough validation
of its effectiveness. The performance of our approach was benchmarked
against established baseline methods, which primarily rely on geometry-based
traversability predictions. The results demonstrated that the deep neural network,
trained with simulation data and using LIDAR and IMU inputs, significantly
improved the accuracy of terrain traversability assessments dynamically adjusted
to the direction in which the robot heads, a limitation often overlooked in
the state-of-the-art. This marks a substantial advancement in autonomous
navigation capabilities in challenging outdoor terrains, where dynamic and
robust traversability assessments are crucial.

4.6 Paper VI: Enabling Robot Autonomy through a Modular

Software Framework
Paper VI, "Enabling Robot Autonomy through a Modular Software Framework,"
focuses on creating a flexible software architecture for robotic navigation,

31
4. Paper Summaries

particularly in uneven terrains. It is motivated by the need for a versatile

framework that can adapt to various environmental representations and robot
types. The paper addresses challenges in robotic navigation by integrating
state-of-the-art motion planning algorithms with ROS 2, allowing customization
without altering the core planners. This is particularly beneficial for dynamic
environments, such as those encountered in agricultural applications. The
framework’s adaptability and efficiency in diverse settings represent its key
contributions. The components include the ROS 2 framework, OMPL for motion
planning, and modules for adapting to different robot types and environment
representations. These components work together to enable a flexible, adaptable
system for robotic navigation, particularly in dynamic and uneven terrains,
without modifying the core motion planning algorithms.
The method demonstrated superior navigation performance in outdoor
scenes, especially on uneven terrains, compared to the widely used Navigation 2
framework. The proposed framework outperformed Navigation 2 regarding
navigation success and adaptability to differing robots in uneven terrains,
highlighting its suitability for mobile robots operating under diverse kinematic
constraints.

32
Chapter 5

Discussion
In this chapter, we articulate several critical conclusions derived from the
research conducted within this thesis. These conclusions are informed by a
thorough interpretation of our findings, substantiated by the experimentation
and validation of the methods we proposed. The core focus areas of our
research—motion planning, environmental representations, and traversability
estimation—have been instrumental in facilitating robust navigation over uneven
terrains. These fundamental themes have not only deepened our understanding
of the inherent challenges in autonomous outdoor navigation but have also
guided us toward innovative solutions. The insights gained have led to a series
of focused discussions on a variety of relevant topics, which we will explore in
the subsequent sections here.

5.1 The Motion Planning Problem in Outdoor Uneven

Terrains
This thesis delved into the nontrivial problem of motion planning in uneven
terrains, a subject thoroughly explored in several of its constituting papers.
Sampling-based planners emerge as a promising option for outdoor robots,
which often traverse extensive areas. The challenge lies in representing these vast
operational spaces volumetrically with fine resolution, as this can lead to memory
and performance inefficiencies. We have demonstrated that raw point cloud
maps, typically generated by state-of-the-art Simultaneous Localization and
Mapping (SLAM) methods, are not only an effective choice for environmental
representation but also a powerful tool in guiding the planning process. These
maps assist sampling-based planners in selecting samples more judiciously,
thereby enhancing the efficiency and efficacy of the motion planning process in
complex outdoor environments. The efficacy of motion planning approaches by
thesis was emphasized by the findings presented in Papers II and III. However,
subsequent to these publications, it became evident that robust motion planning
hinges critically on accurate traversability estimation. This realization emerged as
a key motivating factor driving the research direction following the presentation
of Papers II and III. The importance of precise traversability assessments in
enhancing the robustness of motion planning algorithms, especially in complex
and uneven terrains, became a focal point of our continued investigations.

5.2 The Need for Exploitation of Terrain

The research in Papers II and III demonstrates that active exploration and
exploitation of terrain characteristics significantly expedite the motion planning

33
5. Discussion

process. This is particularly crucial when employing sampling-based planners,

where the strategic selection of samples is essential. Samples must be chosen from
the uneven terrain surface to ensure a realistic and feasible motion plan. This
approach of terrain exploitation in the sampling process not only streamlines the
search for viable paths but also aligns the planning process more closely with
the physical constraints and realities of the outdoor environments.

5.3 The Adaptability of Different Robot Platforms to Uneven

Terrains
In pursuit of developing robot-agnostic navigation solutions for uneven terrains,
it is imperative to incorporate adaptability in motion planning and traversability
estimation stages. Our research demonstrates that facilitating a seamless
transition between robots with different kinematic models is achievable by
utilizing various state spaces offered by the Open Motion Planning Library
(OMPL). These include Special Euclidean 3 (SE3) and SE2 for general motion
and Dubins or Reeds-Sheep paths, which are particularly suited for Ackerman-
steered robots. Furthermore, introducing the ’Elevation’ state space is a crucial
innovation in our work. This state space accommodates the constraints of
traversable ground surfaces, enhancing the motion planning algorithm’s ability
to adapt to diverse robotic platforms and the complexities of uneven terrain
navigation. The adaptability of traversability predictions to various robotic
platforms is a focal point in Papers II and V. In these studies, we aimed
to transcend the conventional geometric terrain analysis for traversability
assessment. Instead, we approached this as a regression problem, where a
neural network estimates the traversability cost of the surrounding terrain. This
estimation is based on the robot’s previous experiences, effectively leveraging past
data to inform current decision-making. By framing traversability prediction in
this manner, we aimed to create a more dynamic and context-sensitive model
that adapts to different robots’ unique capabilities and experiences, enhancing
their navigational efficiency in diverse and challenging terrains. Nevertheless, a
notable limitation of this approach is the necessity to accumulate navigational
’experience’ for each robot with distinct navigation capabilities. This requirement
poses a significant challenge, as it demands the collection of extensive, robot-
specific data to train the neural network effectively. Such a prerequisite can be
resource-intensive and may limit the scalability of the method when applied to a
wide range of robotic platforms with varying characteristics and capabilities.

5.4 Kinodynamic Motion Planning for Uneven Terrains

While geometric motion planning has been at the forefront of recent research in
the field, kinodynamic motion planning presents unique challenges, especially
in outdoor environments. Robots operating outdoors must consider not only
geometrically feasible and optimal paths but also adhere to the feasible range of
their control inputs, like velocity limits, steering rates, and torque constraints.

34
 Going Beyond Geometrical or Visual-based Traversability Estimation

These additional constraints significantly reduce a robot’s maneuverability,

adding complexity to the motion planning problem. Our research confirms that
incorporating heuristic-like functionalities into motion planners can enhance their
efficiency, increasing the likelihood of finding faster solutions with reduced overall
path costs. This is evidenced in Paper IV through the experimental validation of
our proposed kinodynamic motion planner ’CostTrust.’ Its approach aligns with
state-of-the-art geometric motion planners that employ ’informed sampling’ as a
heuristic for quicker convergence to a solution. Despite outperforming other state-
of-the-art kinodynamic motion planners, ’CostTrust’ does encounter challenges.
Its highly probabilistic nature means that in some instances, particularly in
’unlucky’ runs, adding more samples to the tree structure can paradoxically
increase the difficulty of finding a solution, a challenge worth investigating
further.

5.5 Going Beyond Geometrical or Visual-based

Traversability Estimation
In the existing literature, traversability estimation has primarily been approached
as a value estimation problem using geometrical or visual information from robotic
sensors. However, a novel paradigm is emerging that leverages the additional
dimension of a robot’s locomotion history. This method associates unexpected
locomotion patterns with specific terrain types, enabling robots to distinguish
terrains likely to cause disruptions in locomotion, such as obstacles or steep
ramps. In Paper V, we demonstrate that this approach can significantly enhance
the training of neural networks for traversability estimation. This pipeline allows
for loosely supervised training, where data samples and labels are generated
through simple automated programs, eliminating the need for manual labeling.
This technique inherently produces dynamic traversability estimates, effectively
recognizing the varying costs associated with different terrain inclinations and
making the estimates sensitive to the robot’s direction of travel. However,
a current limitation of this method is its non-transferability across different
robotic platforms. Each robot’s unique experience and locomotion characteristics
necessitate the re-collection of data for individual platforms, posing a challenge
for broader applicability.

5.6 A Holistic Approach to Uneven Terrain Navigation

The realm of uneven terrain navigation has witnessed the introduction of
numerous algorithms, each targeting specific aspects such as motion planning,
control, and environmental representation. However, a significant challenge
arises when attempting to benchmark or compare these novel methods against
the existing literature, especially within the context of practical navigation
experiments. A notable gap in the field is the absence of a comprehensive
navigation framework tailored for uneven terrain, which could seamlessly
integrate various components like motion planners, controllers, and environmental

35
5. Discussion

representations. To mitigate this gap and enhance the practicality and reusability
of the algorithms presented in this thesis, we have developed ’vox_nav.’ This
uneven terrain navigation framework is designed with a plugin-based architecture,
accommodating a wide range of motion planners, controllers, and environmental
representations. ’vox_nav’ is versatile, supporting motion plans in state spaces
such as SE3, SE2, Dubins, Reeds-Sheep, and Elevation (ours). The framework
includes 2 map plugins, 5 motion planner plugins, and 3 controller plugins,
and is compatible with the majority of state-of-the-art motion planners from
OMPL. While ’vox_nav’ has been rigorously tested on the robot platforms
and environments depicted in Figure 3.3, this presents both a strength and a
limitation. Its effectiveness has been validated on real systems, yet for it to
be established as a standard in the field, more extensive testing across various
robotic platforms and in a wider array of environments is necessary. This
broad-based validation is essential to confirm its adaptability and effectiveness
in diverse navigation scenarios.

36
Chapter 6

Conclusion and Future Work

This thesis represents a stride forward in the domain of uneven terrain
navigation, encompassing a broad spectrum of advancements in motion
planning, environmental representation techniques, traversability estimation,
and kinodynamic motion planning. Each component addressed within this thesis
highlights the criticality of effective navigation across uneven terrains. Our
research underscores the vital role of actively exploiting terrain information
to achieve optimal motion plans in challenging environments. Beyond the
scope of traditional geometric motion planning, our work advances the field of
kinodynamic motion planning. This aspect is particularly crucial for outdoor
robots, which often operate under stringent dynamic constraints, such as steering,
velocity, or acceleration limits. These constraints inherently reduce the system’s
maneuverability, complicating the navigation process. Our contributions offer
essential insights and methodologies for navigating the complexities inherent
in uneven terrain navigation, thereby augmenting the efficacy and safety
of outdoor robotic systems in such environments. A key discovery in our
research is the crucial role of active terrain exploitation, which has steered
our efforts toward developing neural network-based dynamic traversability
estimation. These estimates are uniquely configured to be cognizant of the
robot’s specific capabilities and directional orientation, as the learning process
fundamentally depends on the robot’s navigational experiences. This adaptability
is instrumental in devising collision-free motion plans and in implementing
effective control mechanisms. By integrating the robot’s interaction with its
environment into the learning model, our approach enhanced the precision and
reliability of motion planning, ensuring a higher degree of safety and efficiency
in complex navigation tasks. This advancement in dynamic traversability
estimation enhances the precision of motion planning and ensures a higher
degree of adaptability and responsiveness in real-world navigation scenarios,
making it an indispensable component in robotic navigation.
Another key achievement of this work is the development of a comprehensive
navigation framework that highlights the practical applicability of our presented
algorithms. This framework is designed with a standardized, plugin-based
interface, which facilitates the integration of new algorithms while maintaining
seamless interoperability among various components such as controllers, motion
planners, and environmental collision checkers. Such a holistic approach not
only demonstrates the effectiveness of the proposed methodologies in real-world
scenarios but also sets a precedent for future research and development in the
field. By providing a flexible and adaptable platform, this thesis contributes to
the ongoing evolution of navigation systems in complex and uneven terrains,
ensuring that they remain responsive to emerging methods.

37
6. Conclusion and Future Work

6.1 Future Directions

While this thesis has investigated numerous specific challenges associated with
ground robots navigating over uneven terrains, the field still presents a myriad
of themes ripe for further exploration. The complexities inherent in uneven
terrain navigation for ground robots create a fertile ground for advancing current
methodologies, developing new technologies, and exploring uncharted aspects of
robotic navigation.

6.1.1 The Terrain-Robot Reaction Estimation

A critical area for future exploration is the estimation of terrain-robot interactions,
particularly in extreme outdoor conditions such as environments with water
puddles, non-compact soil with the potential for sinking, or areas with tall grass.
Accurately predicting how the terrain will react upon contact with a robot could
significantly enhance safety and reliability in uneven terrain navigation. This
endeavor could be advanced through the use of visual sensors like RGB cameras,
thermal imaging, or multispectral cameras for soil parameter estimation. The
integration of these technologies promises to provide a more comprehensive
understanding of terrain dynamics, enabling robots to navigate more effectively
and safely in challenging outdoor environments.

6.1.2 Advanced Sensory Fusion for Terrain-Robot Interaction

Analysis
Building on the idea of terrain-robot reaction estimation, future research could
focus on advanced sensory fusion techniques that integrate data from various
sensors like RGB cameras, thermal imaging, LIDAR, and ground-penetrating
radar. This fusion would enable a more sophisticated understanding of terrain
properties, such as moisture content, density, and texture variations. The goal
would be to develop algorithms that not only estimate the reaction of the terrain
to the robot but also predict potential alterations to the terrain caused by
environmental factors like rainfall, temperature changes, or human activities.
This research could be pivotal in applications such as precision agriculture,
environmental monitoring, and planetary exploration.

6.1.3 Increasing the Adaptability to Dynamic Environments

Expanding upon our work with sampling-based motion planners and environmen-
tal representations, a promising direction for future research is creating adaptive
algorithms capable of dynamically responding to evolving or unpredictable en-
vironments. This line of inquiry would be particularly relevant in scenarios
characterized by frequent environmental shifts, such as those encountered in
disaster response operations or military reconnaissance missions. Integrating
machine learning techniques to predict environmental variations and correspond-
ingly adjust motion planning strategies in real time could substantially improve

38
 Future Directions

outdoor terrain navigation systems’ robustness and overall effectiveness. Such

advancements would enhance the adaptability of these systems and broaden
their potential applications in complex and fluid environments.

6.1.4 A Push Towards Mapless Navigation

Within the scope of this thesis, most comprehensive navigation strategies either
necessitate the construction of an environmental map or operate under the
assumption of its pre-existence. While effective, this approach significantly
escalates human involvement and can be less adaptable to dynamic environments
where changes render pre-built maps obsolete. To circumvent these challenges,
a promising direction for future research lies in the development of navigation
methods that either eliminate the need for a detailed map or rely on a minimal
set of landmarks or environmental topology. For instance, utilizing open-source
mapping databases such as Open Street Map (OSM) to acquire basic road
network structures could be a viable approach. Such mapless or minimally-
mapped navigation strategies hold the potential to enhance the autonomy of
robots, making them more adaptable and capable of operating effectively in
dynamically changing environments.

6.1.5 Development of Reactive Navigation Systems Based on

Real-Time Environmental Feedback
Extending on the idea of mapless navigation, a future direction could be the
creation of reactive navigation systems that utilize real-time environmental
feedback instead of relying on static maps or minimal landmarks. This approach
would involve the development of algorithms that can interpret and react
to immediate surroundings using a combination of sensor inputs, AI-based
scene understanding, and predictive modeling. These systems would be highly
beneficial in environments that are either too dynamic or too poorly mapped
for traditional navigation systems, such as in search and rescue missions in
disaster-struck areas or navigation in dense forest environments.

6.1.6 Hybrid Navigation Models Combining Rule-Based and

Machine Learning Approaches
To enhance adaptability to dynamic environments, a novel approach could involve
the development of hybrid navigation models that blend rule-based systems with
machine learning techniques. Such models could utilize rule-based algorithms for
structured environments and machine learning-based approaches for unstructured
or highly dynamic settings. The integration of these two methodologies would
provide a balance between the predictability and reliability of rule-based systems
and the adaptability and learning capabilities of AI-driven models. This hybrid
approach could be particularly effective in environments where conditions change
rapidly, such as in urban search and rescue operations or military applications
in varied terrains.

39
Papers
 I
Paper I

Benchmark of Sampling-Based
Optimizing Planners for Outdoor
Robot Navigation

Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

Intelligent Autonomous Systems 17. IAS 2022. https://doi.org/10.1007/978-3-
031-22216-0_16.

43
 Benchmark of Sampling-Based Optimizing
Planners for Outdoor Robot Navigation

Fetullah Atas1 , Grzegorz Cielniak2 , and Lars Grimstad1

1
Faculty of Science and Technology, Norwegian University of Life Sciences,
Aas, Norway,
fetullah.atas@nmbu.no
2
Lincoln Centre for Autonomous Systems, University of Lincoln, UK
gcielniak@lincoln.ac.uk

Abstract. This paper evaluates Sampling-Based Optimizing (SBO) plan-

ners from the Open Motion Planning Library (OMPL) in the context of
mobile robot navigation in outdoor environments. Many SBO planners
have been proposed, and determining performance differences among
these planners for path planning problems can be time-consuming and
ambiguous. The probabilistic nature of SBO planners can also compli-
cate this procedure, as different results for the same planning problem
can be obtained even in consecutive queries from the same planner. We
compare all available SBO planners in OMPL with an automated plan-
ning problem generation method designed specifically for outdoor robot
navigation scenarios. Several evaluation metrics are chosen, such as the
length, smoothness, and success rate of the resulting path, and proba-
bility distributions for metrics are presented. With the experimental re-
sults obtained, clear recommendations on high-performing planners for
mobile robot path planning problems are made, which will be useful to
researchers and practitioners in mobile robot planning and navigation.

Keywords: Sampling-Based Optimal planning, Path Planning Bench-

mark, Outdoor Robot Navigation.

1 Introduction

Planning is a core component of an autonomous robot’s software stack alongside

perception and control. It is a process of estimating a series of consecutive, valid,
collision-free states (or poses) that a robot can follow to safely get to the desired
pose. Sampling-Based Optimal (SBO) planning has been a popular approach
in recent years due to its effectiveness in planning in complex, high-dimensional
state spaces typical in robotic manipulation scenarios. SBO planners aim to min-
imize or maximize a given objective function. The ultimate goal is to get a good
plan with the lowest cost or highest reward depending on the selected objective
function. Unlike grid-cell-based methods, SBO planners efficiently explore high-
dimensional state spaces using various sampling strategies. Most of the time, the
objective function is a cost function, where the amount of cost depends on the

45
 Fig. 1. Figure illustrates representative planning results from 16 distinct SBO planners
in a real-world setting for a randomly generated planning problem. The dark blue
points indicate various obstacles such as buildings, the colored paths generated by
each planner are displayed with sequential arrows.

path’s different properties, such as its length, smoothness, or estimated energy

consumption along the path.
Since the breakthrough of SBO planners, initiated in [14], a large variety of
SBO planners have been proposed. Most of these planners are built on top of
RRT* and PRM* planners (e.g., RRTX, LazyPRM* InformedRRT*, etc.), more
recently, another set of planners makes use of a structure that includes both
graphs and trees (e.g., BIT*, ABIT*, AIT*). Some methods use parallelized ap-
proaches (e.g., CFOREST, AnytimePartShortening(APS)), where multiple in-
stances of planners are run on different threads simultaneously while planners
inform each other of the milestones they achieve. Out of all of these methods, it’s
critical to shed some light on the performance of various planner groups and their
characteristics for outdoor robot navigation. SBO planner’s efficient space explo-
ration (by different sampling strategies) suits large-scale outdoor robot planning
while also considering the kinematic constraints. There is a large variety of SBO
planners that could be used for outdoor robot planning. However, there is lit-
tle known about the performances of all these planners. Therefore it is vital to
systematically benchmark and identify better performing planners specifically
for outdoor robot planning tasks. The lack of work in literature that focuses on
SBO path planning performance analysis, particularly for outdoor robot path
planning, has been the motivation behind this work. The main contributions of
this work are listed as follows:

– We propose a planner benchmarking methodology and experimental results.

We have provided clear recommendations on the most suitable planners for
outdoor robot motion planning scenarios.

46
 – We use an automated planning problem generation method that can better
capture the behavior of probabilistic SBO planners by observing results from
many different planning problems.
– We present a performance analysis of state-of-the-art SBO planners with
realistic planning times in real and simulated 3D environments.

2 Related Work

The number of SBO planners has increased steadily in the past decade. As this
is the case, several benchmarking works have been proposed to investigate the
performances of each planner in-depth for specific scenarios. Meijer et al. [19],
investigated various OMPL planners for grasping execution tasks on different
robotic arms. Based on critics such as; the number of exactly solved runs, path
length, and computation times, recommendations were made for high-performing
planners depending on the type of grasp execution, e.g., pick or place. Jedrze-
jczyk et al. [13] presented an empirical work focusing on benchmarking of vari-
ous planners within MoveIt! framework [3]. The particular focus of the planning
problem was related to a robotic harvester arm. The task was picking toma-
toes in a simulated greenhouse environment. The authors used time of planning,
time of generated plan simplification, and time plan execution as metrics for
performance evaluation. The results for metrics are provided as average however
it is not clear how many runs were performed to obtain the average values for
metrics. Magalhaes et al. [18] benchmarked OMPL planners on a six-degree of
freedom manipulator in a simulated vineyard for pruning tasks. The authors
use the success rate, path length, planning time, and path clearance as their
metrics. The authors conclude that most of the evaluated planners in OMPL
perform poorly for demanding pruning tasks in agricultural setups.
Commonly, the above works focused on robotic manipulation scenarios; they
used path length, success rate, and execution time as evaluation metrics. In our
benchmark, we exclude the execution time from the metrics. We solely bench-
mark optimizing planners as we believe that the quality of generated path for an
outdoor robot is crucial. The optimizing planners utilize all allowed time to get
the most optimal path. Therefore time consumed by each planner is the same.
We further discuss on this in Sec. 3.1.
More recent work by Heiden et al. [10] introduced a benchmarking tool for
path planning of mobile robots. The authors present the Bench-MR platform for
convenient performance evaluation of different sampling-based and grid-based
path planning algorithms. To showcase the features of the proposed tool, the
authors provide sample benchmarks from several planning algorithms. However,
there is no large-scale comparison of different planners.
Moll et al. introduced a generic software infrastructure wrapping around the
OMPL interface in [20], which allows straightforward comparison of OMPL plan-
ners considering different performance metrics. The tool has introduced many
performance metrics making it convenient to evaluate performance for additional
requirements. We have used this benchmarking tool to carry out experiments.

47
 As can be seen from highlighted summaries above, the existing works on
sampling-based planning benchmarking have focused on benchmarking planners
for robotic manipulation. We also realize a lack of methodology that can re-
veal more profound performance differences of probabilistic planners that are
being evaluated. We deal with these shortcomings by introducing a method (see
Sec. 3.2). We also use an automated problem generation on the real and simu-
lated environment that can generate a large number of valid planning problems.
Both of these allow us to gain a clear picture of SBO planners’ performance in
the setting of unstructured outdoor path planning.

3 Problem Formulation

We highlight planners subject to evaluation in Tab. 1 and describe their unique

characteristics shortly. All of the optimizing planners within OMPL are selected
for evaluation. The reason for only choosing optimizing planners is that the
quality of the acquired path is crucial for a mobile robot. An acquired path will
not only be expected to be valid but also be shorter, smoother, and take less
computation time to ensure minimal energy consumption and risk during the
path following action.
The task of path planning is to find a set of consecutive states Si that connects
a start state Ss to a goal state Sg . All of the states belong to a physical space that
the robot operates in; this will be called Workspace W . Within the Workspace
W , a parameter space represents all possible state configurations of the robot,
State-space S. However, states present in S must meet a condition to be valid.
A state will be valid only if it does not collide with the physical space. Therefore
final path must be in a subset state-space named collision-free state-space Sf ree .
We can formally define a valid solution path, θ based on the above definitions.

θ(0) = Ss , θ(1) = Sg ,
(1)
θ(x) ∈ Sf ree , ∀x ∈ [0, 1]

To account for sampling-based optimal path planning, we must include an

additional definition related to cost function c in Eq. 1
X
c: −→ R≥0 ,
(2)
c(θ∗) = min c(θ), θ(x) ∈ Sf ree , ∀x ∈ [0, 1]

where c(θ∗) is an optimized path W.R.T given cost function c, which assigns
a strictly positive cost to collision-free paths.
In our benchmark, planners will minimize a cost function c, where the cost is
determined by the path’s length and obstacle clearance properties. The obstacle
clearance objective aims to steer the path away from obstacles. A weighted
combination of these two objectives is used for determining cost c. The weights
were set to 10 and 1 for path length and clearance objectives, respectively.

48
 Planner Name Short Description
PRM* [14] Number of nodes to be connected are not fixed but is determined auto-
matically based on coverage of the state space.
LazyPRM* [2] As in PRM*, the number of nodes to be connected is not fixed but is
determined automatically based on coverage of the state space, differently,
the edges of nodes are not immediately checked for the collision but are
checked only when a better path to the goal is found.
RRT* [14] An asymptotically-optimal version of RRT that is guaranteed to converge
to an optimal solution if one exists. The optimality refers to a specified
cost function, commonly a function that determines path length.
RRTsharp [1] Bases on Rapidly-exploring Random Graphs(RRG), it improves the con-
vergence rate by keeping stationary information of sampled vertices; by
doing this, RRTsharp can achieve better estimations of potential optimal
paths.
RRTX [22] A variant of RRT*, the nodes in the tree are rewired only if the cost is
being reduced by a specified margin(known as epsilon), this improves the
convergence of solution.
InformedRRT* [8] Narrow downs tree exploration space to boundaries of a prolate hyper-
spheroid, a prolate hyperspheroid is n-dimensional symmetric ellipse that
is constructed around current optimal plan. As the search space is reduced
to region around optimal plan, the converge rate improves significantly.
BIT* [7] Uses a unified structure that is composed of both graph and trees, this
leads to a few advantages such as being able to use heuristics like one in
A* [9] to prioritize tree expansions towards the goal with high-quality
paths.
ABIT* [27] An extension of BIT*, Advanced BIT*(ABIT*), combines an advanced
graph-based search with an RRT* like planner for faster convergence rate.
AIT* [26] Another variant of BIT*, uses an asymmetric bidirectional search where
both of the searches continuously inform each other, this leads to quicker
initial solutions.
LBTRRT [24] Allows for a continuous interpolation between RRT* and RRG algorithms
for high-quality solutions (comparable to RRG and RRT*) with little
running-time.
SST [16] An enhancement of RRT* which tries to address optimal kinodynamic
planning when there is no access to two-point boundary value prob-
lem(BVP) solver, BVP is required to get the desired property for systems
with dynamics.
SPARS [5] Built on top of PRM*, suggests finite-sized nodes within the roadmap
which might provide near-optimality properties.
SPARStwo [4] Extends on SPARS. Sparser graphs can provide solutions meeting near-
optimality requirements. Reduced resources are used to solve the plan-
ning problem, SPARStwo also decreases the rate of node addition into the
roadmap.
FMT* [12] Utilizes recursive dynamic programming to form a tree from probabilisti-
cally sampled states.
CFOREST [21] Wraps a parallelized framework around single-query SBO planners, e.g.,
RRT*. Separate trees are grown from different threads while these threads
communicate with each other every time a thread finds a better path.
Other threads are also informed, leading to a significant speedup of con-
vergence. The result paths are shorter and smoother.
AnytimePartShortening(APS) [17] A generic method that wraps around one or more planner. The method
applies an iterative path simplification, shortening process. It is capable
of generating a hybrid path if they are to yield shorter solutions.

Table 1. List of evaluated SBO planners and their short descriptions.

3.1 Evaluation metrics

There are various metrics to assess the quality of resulting paths, such as path
length, total execution time, availability of exact solution, etc. SBO planner’s
execution time is quite ambiguous and needs an explanation here. SBO planers
use a timeout parameter; initially, some of the time is used to construct an

49
 initial valid plan (if one exists). After a valid plan is found, the remaining time
is utilized for getting lower-cost paths, at the end, all given time is used. Since
in this work we are interested in optimal paths, we cannot set execution time
as a metric since SBO planners utilize all given time to construct the optimal
plan. In experiments, all planner’s timeout were set to 5 seconds. Metrics of
evaluation and their short descriptions are given as follows.

– Average Solve Rate: Number of exact solutions divided by the total number
of problems.
– Solution Length: Real length of the resulting solution path.
– Solution Smoothness: The smoothness of the path is calculated by the sum
of angles between consecutive path segments. Assume a path as sequence
of segments where three states within the path form two segments. Using
Pythagoras generalized theorem, one can calculate angles between consec-
utive segments for all intermediate states. Each angle is normalized by the
summed lengths of two segments. Summation of all normalized angles gives
final smoothness value for the path. The lower this value the smoother the
path is. For instance, a perfectly straight line will result in a smoothness
value of zero.

3.2 Evaluation Methodology

The planners subject to evaluation are not deterministic but probabilistic. There-
fore from a single run, it is not possible to measure the performance of a planner.
Hence we run planners on a high number of randomly generated planning prob-
lems, we then use normal distributions of results from each run to determine
and rank the performances of planners.
Two-Sample Z-Test is a statistical method to evaluate the differences between
two normal distributions. We use a Two-Sample Z-Test to find out whether the
performance of a planner differs significantly compared to other planners, this
helps us to obtain the proportion of difference from two normal distribution
metrics. Additionally, we can visualize the performance of each planner against
every other planner, as in Fig. 2. The formal equations of the Two-Sample Z-Test
are written as follows;
s
σ12 σ2
s= + 2,
N1 N2
µ1 − µ2
z= ,
s
x − µ1 (3)
1 0.5( )
f (x) = √ e σ1 ,
σ1 2π
Z z
1
P (X ≤ z) = √ f (x), dx
σ1 2π −∞

50
 Where σ1 and σ2 are standard deviations, N1 and N2 are sample sizes and
µ1 and µ2 are mean values of normal distributions for compared planners. Fi-
nally, P (X ≤ z), also named the p-value, determines the area under the normal
distribution and assesses the significance of change between the two compared
distributions. We perform the procedure of Z-Test for each planner against every
other planner(including the planner itself); if the p-value for this comparison is
below a threshold a, the result is 1 and -1 if otherwise. This results in a 2D skew-
symmetric matrix with the size of the number of planners. The values are in the
range of [−1, 1], the column-wise summation of positive values in this matrix
tells how many planners have been outperformed by the planner in each col-
umn(see Fig. 2). We acquire this matrix for both smoothness, and path lengths
metrics since results from these two are presented with distributions. Consider-
ing multiple non-deterministic metric values, it is difficult to withdraw an overall
performance score. Therefore we consider weighting the metrics and summing
them respectively.

T = Srate (wlength Zlength + wsmooth Zsmooth ) (4)

Where T is the final score of the evaluated planner’s performance, Srate is the
average solution rate of the planner(depicted in Table 3). Zlength and Zsmooth
are scores of planners produced by the Two-Sample Z-Test (See the horizontal
axis of Fig. 2). We assume that the length of a path has higher importance
than smoothness on the overall quality of path in the Dubins state space. The
resulting paths in Dubins are smooth enough to respect a robot’s kinematic
constraints. Therefore in Sec. 4 we assume values of wlength and wsmooth as 2
and 1, respectively.

4 Experiments

4.1 Scenarios and Evaluation Setup

The environment is represented with an OctoMap [11], a memory-efficient tree

data structure that holds 3D occupancy information of the environment in a
probabilistic manner. Collision checking is performed between a 3D bounding
box that represents the robot body and a prebuilt OctoMap of the environment.
For the collision check, we use the Flexible Collision Library(FCL) [23]. We
evaluate planners on two different maps, one acquired from the Gazebo [15]
simulator and one from a real environment, the map from the real environment
was acquired with a 3D SLAM method named LIO-SAM in [25].
Both simulated and real environments consist of various objects such as build-
ings, lamp posts, plants, etc. The terrain is unstructured, though the simulated
environment includes slightly more aggressive hills. We selected the maps to
be of natural non-designated areas with 3D representations to account for the
navigation of mobile robots in a cluttered outdoor terrains (see Fig. 1).
We use a randomized goal and start pose generation within defined state
space boundaries. The validity of these start and goal poses was acquired by

51
 collision check status between a 3D box representing the robot’s body and Oc-
toMap at start and goal poses. To make sure that an actual valid plan exists
between randomly generated start and goal poses, a planning request was made
from start to goal pose with a sufficient amount of timeout (10 seconds). The
actual benchmarking was initialized only if there existed a valid plan from ran-
dom start to random goal pose, we repeat this process until we achieve 500 valid
pairs of start and goal poses that define 500 unique planning problems.
The planners are evaluated in Dubin [6] state space to meet the kinematic
structure of the Ackerman steering ground robot. Dubin’s state-space particu-
larly suits forward-only motions, adding more challenge to the planning problem.
Dubin’s state space ensures kinematic constraints are complied with by providing
path segments that lay on curves in between start and goal poses. We must note
that each planner’s performance will be reasonable and similar if given enough
time. Therefore we used a strict timeout of 5 seconds to challenge and reveal
performance differences among SBO planners. We generated 500 random plan-
ning problems with the minimum Euclidean distance between start and goal pose
being 30 meters. To reduce bias related to the probabilistic nature of planners,
we requested each planner to solve each problem twice, making the total number
of runs 1000 for each planner. Since we evaluated 16 SBO planners, 16000 runs
were performed, each taking 5 seconds, resulting in a total computation time of
22.22 hours on a laptop with Intel Core i7-6700HQ 2600 MHz CPU with eight
cores and 16GB of memory. All numbers reported in the following sections are
result of 1000 runs.

Fig. 2. A matrix depicting whether one can state that, a planner’s performance in a
column produced significantly shorter paths than a planner in a row with the confidence
of 100 ∗ (1 − a)%. The significance threshold a is set to 0.05. The normalized column-
wise summation of matrix values is obtained on the vertical axis for each planner. In
this case, the APS planner achieves the best score (1.0).

52
 In the following, we will interpret results metric-wise.

4.2 Interpreting Results Metric-Wise

The means and standard deviations of resulting path lengths are presented in
Table 2. It is important to note that these values are acquired only from the
exactly solved runs. For instance, the FMT* planner could not exactly solve any
given problem. Therefore, it is not included in Table 2 and Table 3. Looking at
Table 3, one can see that except for the FMT* planner, each planner’s num-
ber of exact solutions is similar, meaning that the number of samples used to
calculate values in Table 2 is similar for all planners. In Table 2, a lower mean
value indicates a shorter path in the context of the path length metric. Accord-
ing to mean values, APS achieves 25.90% shorter paths than the rest of the
planners, while SPARS achieves the longest paths. Both APS and CFOREST
use a parallelization(see Tab. 1) trick to initiate multiple instances of a selected
planner and run them separately on parallel threads. These different instances
of planners continuously inform each other of achieved milestones, which seems
to land them significant performance advantages towards more optimal paths
within the given time. This can be seen from the horizontal axis of Fig. 2 and
Table 2. Planners from RRT* and BIT* families perform similarly, except AIT*
planner, which performs better than (6.59% shorter paths on average) the rest
of the planners from RRT* and BIT* families, thanks to its asymmetric bidi-
rectional search technique. Planners from the PRM* family, such as SPARS and
SPARStwo, produced the longest paths. We argue that this is due to the plan-
ner’s stricter attitude towards adding more nodes to the roadmap. We observed
from a sample run that PRM*, a predecessor of SPARStwo, added 799 nodes to
the roadmap while SPARStwo added only 30 nodes to the roadmap, leading to
longer and less smooth paths overall.
The APS planner achieves the best performance in the path smoothness met-
ric, while SST achieves the worst. We notice that, on average, the APS planner
creates five times smoother paths compared to the next best performing planner
CFOREST. Unlike in path length metric, planners of RRT* family outperform
planners of BIT* family in path smoothness metric (see Table 2). Similar to their
performance in path length metric, PRM* family planners do not particularly
create smoother paths, making them average performers. From Table 2, we see
that tree-based planners create smoother plans compared to graph-based plan-
ners. This makes sense since a tree-based planner adds the next sample state
to a relevant pose within the tree structure whereas, in a graph-based planner,
the next sample state could be placed to a less relevant pose, causing increased
curvature of the final path.

4.3 Combining The Metrics for Absolute Ranking

We use the proposed methodology described in Sec. 3.2 to achieve an absolute
performance score for each planner. It must be noted that we assume a higher
weight for the path length metric. We assume that the path length importance

53
 is more significant for a mobile robot, still, it is possible to use different weights
and recalculate final scores with given information in Table 2 and Table 3. As
seen in the vertical label of Fig. 2, the score for each planner has been calculated
by normalized column-wise summation, these accumulative scores are created
for path length and smoothness metrics, which are denoted with Zlength and
Zsmooth respectively.

Planners Length Mean Length Dev. Smoothness Mean Smoothness Dev.

APS 51.8 15.3 0.001 0.004
CForest 54.7 16.3 0.005 0.008
RRTX 56.7 17.0 0.006 0.012
RRTsharp 56.5 17.1 0.006 0.012
RRT* 57.5 17.8 0.007 0.011
InfRRT* 57.6 18.1 0.008 0.011
AIT* 54.7 19.2 0.021 0.025
BIT* 56.6 17.2 0.016 0.021
ABIT* 56.6 17.5 0.017 0.020
LazyPRM* 60.0 19.4 0.013 0.021
PRM* 65.1 22.5 0.039 0.059
LBTRRT 67.2 21.2 0.048 0.053
SPARStwo 92.3 41.6 0.024 0.027
SPARS 98.1 44.6 0.035 0.040
SST 79.6 30.6 0.439 0.281
FMT* - - - -

Table 2. Means and deviations of acquired path length and smoothness metric from
1000 runs. A lower mean value indicates that planners achieve better performance for
both metrics.

Planner Name Average Solve Rate Overall Performance Score

APS 0.99 7.96
CForest 0.97 4.31
RRTX 0.94 3.14
RRTsharp 0.96 3.03
RRT* 0.96 2.94
InfRRT* 0.95 2.80
AIT* 0.93 2.06
BIT* 0.94 1.99
ABIT* 0.94 1.96
LazyPRM* 0.94 1.84
PRM* 0.99 1.05
LBTRRT 0.97 0.90
SPARStwo 0.97 0.78
SPARS 0.95 0.42
SST 0.92 0.37
FMT* - -

Table 3. A table presenting the average solve rate and overall performance scores of
planners calculated by the proposed method in Sec. 3.2. The planners are ranked with
highest final score values.

54
 With acquired Zlength and Zsmooth , Using Eq. 4, we achieve the final per-
formance scores presented in Table 3. Note that the values present in Table 3
are achieved by comparing planners with each other, a low value does not nec-
essarily mean a planner has failed to produce the desired result, but it instead
means that, the planner has performed worse than other planners with greater
final score values. Our primary focus in this paper was on the problem of mobile
robot path planning with kinematic constraints. Our recommendation for users
is to consult to the final scores of planners given in Table 3. Based on these
results, one can judge the most suitable planners for mobile robot navigation on
outdoor terrains. We were not able to acquire any exact solution from FMT*
planner. It is unclear what might have caused this. More investigation for this
particular planner is needed.

5 Conclusion

We evaluated all SBO planners in the OMPL with a systematic approach. We run
each planner on 500 different planning problems twice, resulting in 1000 results
that reveal each SBO planner’s behavior. The evaluation setup was mainly aimed
at the planning of outdoor mobile robots. The proposed evaluation methodology
identifies the most suitable planners for planning in unstructured terrains. With
provided probability distributions of metrics and their combinations, the method
revealed performance differences among planners numerically. The APS planner
was found to be the most consistent planner along with all metrics. Therefore we
recommend the readers’ APS planner, as it is the best performing SBO planner
for mobile robot path planning. This paper does not propose a methodology to
find optimal configuration parameters for each evaluated planner automatically.
There are hundreds of parameters for all planners being assessed, making it
challenging to find optimal values for each parameter. Therefore all planners
were set for experiments with their default parameters. We plan to investigate
parameter tuning for each planner methodologically in future works.

References

1. O. Arslan and P. Tsiotras. Use of relaxation methods in sampling-based algorithms

for optimal motion planning. In 2013 IEEE International Conference on Robotics
and Automation, pages 2421–2428, 2013.
2. R. Bohlin and L. Kavraki. Path planning using lazy prm. volume 1, pages 521 –
528 vol.1, 02 2000.
3. D. Coleman, I. A. Sucan, S. Chitta, and N. Correll. Reducing the barrier to entry
of complex robotic software: a moveit! case study. CoRR, abs/1404.3785, 2014.
4. A. Dobson and K. E. Bekris. Improving sparse roadmap spanners. In 2013 IEEE
International Conference on Robotics and Automation, pages 4106–4111, 2013.
5. A. Dobson, A. Krontiris, and K. E. Bekris. Sparse roadmap spanners. In E. Fraz-
zoli, T. Lozano-Perez, N. Roy, and D. Rus, editors, Algorithmic Foundations of
Robotics X, pages 279–296, Berlin, Heidelberg, 2013. Springer Berlin Heidelberg.

55
 6. L. E. Dubins. On curves of minimal length with a constraint on average curvature,
and with prescribed initial and terminal positions and tangents. American Journal
of Mathematics, 79(3):497–516, 1957.
7. J. D. Gammell, T. D. Barfoot, and S. S. Srinivasa. Batch informed trees (bit*):
Informed asymptotically optimal anytime search. The International Journal of
Robotics Research, 39(5):543–567, 2020.
8. J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot. Informed rrt*: Optimal incre-
mental path planning focused through an admissible ellipsoidal heuristic. CoRR,
abs/1404.2334, 2014.
9. P. Hart, N. Nilsson, and B. Raphael. A formal basis for the heuristic determination
of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics,
4(2):100–107, 1968.
10. E. Heiden, L. Palmieri, L. Bruns, K. O. Arras, G. S. Sukhatme, and S. Koenig.
Bench-mr: A motion planning benchmark for wheeled mobile robots. IEEE
Robotics and Automation Letters, 6(3):4536–4543, 2021.
11. A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard. Octomap:
An efficient probabilistic 3d mapping framework based on octrees. Autonomous
Robots, 2013.
12. L. Janson and M. Pavone. Fast marching trees: a fast marching sampling-based
method for optimal motion planning in many dimensions - extended version.
CoRR, abs/1306.3532, 2013.
13. F. Jedrzejczyk, J. Bajer, G. Gawdzik, J. Glówka, and A. Sprońska. Benchmark
and analysis of path planning algorithms of “ros moveit!” for pick and place task
in tomato harvesting. In R. Szewczyk, C. Zieliński, and M. Kaliczyńska, editors,
Automation 2021: Recent Achievements in Automation, Robotics and Measurement
Techniques, pages 272–284, Cham, 2021. Springer International Publishing.
14. S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion plan-
ning. CoRR, abs/1105.1186, 2011.
15. N. Koenig and A. Howard. Design and use paradigms for gazebo, an open-source
multi-robot simulator. In 2004 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS) (IEEE Cat. No.04CH37566), volume 3, pages 2149–
2154 vol.3, 2004.
16. Y. Li, Z. Littlefield, and K. E. Bekris. Asymptotically optimal sampling-based
kinodynamic planning, 2016.
17. R. Luna, I. A. Şucan, M. Moll, and L. E. Kavraki. Anytime solution optimization
for sampling-based motion planning. In 2013 IEEE International Conference on
Robotics and Automation, pages 5068–5074, 2013.
18. S. Magalhães, F. Neves Dos Santos, R. Martins, L. Rocha, and J. Brito. Path
Planning Algorithms Benchmarking for Grapevines Pruning and Monitoring, pages
295–306. 08 2019.
19. J. Meijer, Q. Lei, and M. Wisse. Performance study of single-query motion plan-
ning for grasp execution using various manipulators. In 2017 18th International
Conference on Advanced Robotics (ICAR), pages 450–457, 2017.
20. M. Moll, I. A. Şucan, and L. E. Kavraki. Benchmarking motion planning algo-
rithms: An extensible infrastructure for analysis and visualization. IEEE Robotics
& Automation Magazine, 22(3):96–102, September 2015.
21. M. Otte and N. Correll. C-forest: Parallel shortest path planning with superlinear
speedup. IEEE Transactions on Robotics, 29(3):798–806, 2013.
22. M. Otte and E. Frazzoli. Rrtx: Real-time motion planning/replanning for environ-
ments with unpredictable obstacles. In WAFR, 2014.

56
23. J. Pan, S. Chitta, and D. Manocha. Fcl: A general purpose library for collision
and proximity queries. In 2012 IEEE International Conference on Robotics and
Automation, pages 3859–3866, 2012.
24. O. Salzman and D. Halperin. Asymptotically near-optimal RRT for fast, high-
quality, motion planning. CoRR, abs/1308.0189, 2013.
25. T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela. Lio-sam:
Tightly-coupled lidar inertial odometry via smoothing and mapping. In IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), pages 5135–
5142. IEEE, 2020.
26. M. P. Strub and J. D. Gammell. Adaptively Informed Trees (AIT*): Fast asymp-
totically optimal path planning through adaptive heuristics. In Proceedings of
the IEEE International Conference on Robotics and Automation (ICRA), pages
3191–3198, 31 May – 31 Aug. 2020.
27. M. P. Strub and J. D. Gammell. Advanced bit* (abit*): Sampling-based planning
with advanced graph-search techniques. 2020 IEEE International Conference on
Robotics and Automation (ICRA), May 2020.

57
Paper II

Elevation State-Space:
Surfel-Based Navigation in Uneven
Environments for Mobile Robots II
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
10.1109/IROS47612.2022.9981647

59
 Elevation State-Space: Surfel-Based Navigation in Uneven
Environments for Mobile Robots
Fetullah Atas 1 Grzegorz Cielniak
Abstract—This paper introduces a new method for robot
motion planning and navigation in uneven environments through
a surfel representation of underlying point clouds. The proposed
method addresses the shortcomings of state-of-the-art navigation
methods by incorporating both kinematic and physical con-
straints of a robot with standard motion planning algorithms
(e.g., those from the Open Motion Planning Library), thus
enabling efficient sampling-based planners for challenging uneven
terrain navigation on raw point cloud maps. Unlike techniques
based on Digital Elevation Maps (DEMs), our novel surfel-based
state-space formulation and implementation are based on raw
point cloud maps, allowing for the modeling of overlapping
surfaces such as bridges, piers, and tunnels. Experimental results
demonstrate the robustness of the proposed method for robot
navigation in real and simulated unstructured environments. The
proposed approach also optimizes planners’ performances by
boosting their success rates up to 5x for challenging unstructured
terrain planning and navigation, thanks to our surfel-based
approach’s robot constraint-aware sampling strategy. Finally, we
provide an open-source implementation of the proposed method
to benefit the robotics community.
Index Terms—Unstructured Terrain Navigation, Outdoor
Robotics, Planning.
I. I NTRODUCTION
There is an increasing demand for robots in uneven en-
vironments such as construction, rural, disaster, or planetary
discovery sites. Current methods for autonomous navigation in
these environments use various environment representations,
including 2D grid maps [7], [8], Digital Elevation Maps
(DEMs) [2], [20], 3D occupancy grids , [19] and meshes [12],
[14].
Despite the variety of proposed methods, some limitations
call for further improvements. A few examples in this regard
are limitations from the environment representation (as in
DEMs for not being able to model some structures such as
tunnels, overhangs, etc.) and the curse of dimensionality in
full 3D occupancy grids. There is also a lack of convergence
towards a standard environment representation for 3D naviga-
tion. 2D grid maps are widely adopted for structured terrain
planning and navigation in academia and industry. As we
explore in Sec. II, there is no widely adopted representation for
navigation in uneven terrains, leading to a need for alternative
1 Norwegian University of Life Sciences {fetullah.atas,
lars.grimstad} @nmbu.no
2 University of Lincoln gcielniak@lincoln.ac.uk
2 Lars Grimstad 1
representations with improvements over existing methods. In
earlier approaches, such as in [11], [12], the standard planning
algorithms were tailored specifically to work with a particular
environment representation requiring additional integration
effort and limited portability between the representations. This
caused a considerable effort to reimplement or adapt differ-
ent planning algorithms to work with proposed environment
representations. This work proposes a new state-space design
and implementation to address these limitations. We built
our method on top of raw point cloud maps. Point clouds
are directly derived from distance measurements provided by
various sensors (e.g., RGBD cameras, LIDARs, etc.). Point
clouds are not volumetric structures that could convey the
occupancy information of the environment. To convey the
occupancy information of the environment, we create an
occupancy volume through surfels, which are introduced as
point primitives to simplify the representation of a dense point
cloud in computer graphics [10]. The surfels hold a cost
property which indicates a traversability measure of the region
they represent within the dense point cloud. The cost property
of surfels is also projected to occupancy volume, see Fig. 2.
The occupancy volume is built where it matters: the traversable
regions on the surface of environments, which is advantageous
since a ground robot’s area of interest is primarily the ground’s
surface. We limit the occupancy volume used for planning
to only relevant locations (ground’s surface) without loss
of ability to model complex structures such as tunnels or
piers, which DEM-based methods cannot model, see Fig. 1.
Finally, together with the cost property, this volume is called
elevation state-space, a dedicated state-space for planning and
navigation in unstructured terrains. We run experiments in real
and simulated unstructured environments to validate our ap-
proach. Through our surfel-based state-space implementation,
we enable seamless integration with the standard planning
techniques such as those from the OMPL [18] library for
unstructured terrain planning and navigation.
In summary, our key contributions are:
• A configurable implementation interface that can account
for different robot types with various kinematic and
physical constraints, we run experiments on Ackerman
and omnidirectional robot types.
• We demonstrate that both in real and simulated uneven
61

Fig. 1: An example use-case for navigation in unstructured
terrain. PRM* planner [5], operating in the proposed state-
space, returns a valid path (in cyan color) by passing under
the pier.
terrains, the proposed approach leads to better results than
standard navigation frameworks (i.e., Navigation2 [7]).
• An open-source implementation of our software 1 which
enables the use of standard motion planning libraries (i.e.,
OMPL) for unstructured terrain planning and navigation,
removing the need for additional efforts to integrate
representation-dependant path planners.
II. R ELATED W ORK
The previous work in mobile robot navigation in unstruc-
tured environments used a variety of map representations,
including digital elevation maps (DEMs), 2D-3D occupancy
grids, and meshes. We take a brief look into recent methods
and explain their characteristics in the following.
A digital elevation map (DEM) is a generic grid map where
each cell within the grid holds the elevation information of
corresponding x and y locations with a function like z =
f (x, y). Wermelinger et al. [20] used a special type of DEM
where, besides elevation, a traversability measure of terrain
was also present within each cell. The authors referred to
the new form of this map as a traversability map. They used
this map for planning paths for a legged robot in unstructured
terrain. Similarly, Fan et al. [2] used a layered grid map where
the elevation and risk factors associated with each position
were present. They then used this layered 2D grid map for risk-
aware planning and navigation in unstructured environments.
These methods rely on an elevation map with limitations such
as not modeling overhangs, tunnels, etc.
An OcTree-based 3D occupancy method, OctoMap, was
introduced by Hornung et al. [4]. In OctoMap, occupancy
1 https://github.com/NMBURobotics/vox nav
62
values of geometric voxels are determined by a probabilistic
approach to account for sensor noise and uncertainties. Wang
et al. [19] presented a framework that indirectly used OctoMap
to navigate an indoor mobile robot in an unstructured envi-
ronment. First, an OctoMap of the domain is created. The
generated OctoMap is then projected to several layers with
various resolutions, and differences of the gained layers are
used to determine the traversability of the environment map.
The planning and navigation are performed on top of this
2D traversability map. The method was developed and tested
only for indoor unstructured environments; hence there was
no incorporation of terrain roughness or ground clearance of
the robot chassis.
Some other methods, such as , [12], [14] have used 3D
meshes generated from 3D point cloud maps of the environ-
ment to represent unstructured terrains. They used acquired
3D mesh edge connectivity to develop a navigation strategy
for unstructured outdoor environments. Authors in [12] use
such a 3D mesh to build the edge connectivity of a 3D envi-
ronment. Graph-based search algorithms are used on this edge
connectivity to create feasible paths for navigation. Although
the method is aimed to be robot-independent, there was no
direct indication for consideration of kinematic constraints,
such as Ackerman-type. Ruetz et al. [14] also presented a 3D
mesh-based framework. However, the acquired 3D mesh was
only based on visible point clouds from the live LIDAR sensor,
limiting the method for global navigation in unstructured
environments.
Although initially not intended for unstructured terrain
navigation, Navigation2 [7] and its predecessor, the original
ROS navigation framework is worth mentioning due to its
popularity in academia and industry [8]. These methods, which
rely on a 2D occupancy grid for environment representation,
are practical and have been used by numerous researchers.
However, because these approaches are based on a 2D occu-
pancy grid for environment representation, their use is limited
for unstructured terrain.
There was less emphasis on the relationship between envi-
ronment representation and planning in the existing methods,
resulting in incompatible, duplicate implementations of path
planning algorithms among used environment representations.
In contrast, our approach places a greater emphasis on the
relationship between environment representation and planning.
We argue that seamless integration between the environment
representation and planning is crucial for challenging unstruc-
tured terrain planning and navigation. Our approach integrates
environment representation and planning tightly through our
surfel-based elevation state-space, eliminating the need to
tailor planners to a specific environment representation. As
we show in Sec. IV, this helps us navigate unstructured
environments effortlessly as we enable access to dozens of
efficient sampling-based planners (e.g., from OMPL).
III. A PPROACH
Surfel is an abbreviation for “surface element” [10], which
is introduced as point primitives to simplify the rendering or
shading of an object represented by a dense point cloud. For
a ground robot navigating in an unstructured terrain where the
environment is represented by such a dense point cloud, the
area of interest is the surface of the point cloud map. We use
these point primitives (surfels) to simplify the representation
of terrain features derived from points, see Sec. III-A. In
the following, we describe the extraction of traversability
information and embedding of this information into elevation
state-space.
An environment is represented with point clouds consisting
of n points. We refer to this point set as the original point
set PO . We perform a uniform sampling to discretize this
environment and distribute the cost values associated with
local patches [15] operation on the original point cloud set
PO . Uniform sampling is performed with a voxel size of ds ,
resulting in a point cloud subset of j points. We refer to
this point set as a sampled point set PS . We extract local
terrain features such as roughness, tilt, the maximum height
of sampled points, and their geometrical relationship with
neighboring points from the original point’s set PO . These
features are used for deriving a traversability measure of the
map, see the itemized cost critics in Sec. III-A.
We construct the surfels with sampled points and their
neighbors within radius rs , from the original point set PO .
Let us refer to this acquired surfel set as the original surfel
set SO , each surfel in this set is defined by a 3D position
vs ∈ R3 , a 3D normal vector ns ∈ R3 and a radius rs ∈ R1 .
The radius of surfel rs is approximated to ds /2.0 to minimize
overlapping where ds is the voxel size. Positions of surfels are
directly acquired from the sampled point’s positions, while the
normal vectors are determined by the RANSAC plane fitting
to the neighboring points of each sampled point. Plane fitting
for points within surfel boundaries is expressed in the plane
equation given in Eq. 1. Then the normal vector for surfel ns
becomes the coefficients of the plane equation:
Ax + By + Cz + D = 0,
(1)
ns = (nsx , nsy , nsz ) = (A, B, C)
A, B, C, D are coefficients of scalar equation of a plane. ns
becomes normal vector for the plane.
A. Regressing Costs on Local Patches of Terrain
We regress the cost values for each surfel in the original
surfel set SO , and this cost defines a traversability measure for
each surfel. We select four critics to determine a cost value
associated with each surfel:
• Tilt of the slope within a surfel, ts .
• Average point deviation from the plane of a surfel, pds ,
is used to determine roughness.
• Max height difference of points within the surfel, hds .
• Ground Clarence to the robot’s bottom chassis, gcs .
These cost critics aim to determine cost values for terrain
patches considering the robot’s physical constraints, e.g., roll
or pitch angle, terrain roughness, ground clearance, etc.
The following equations show how these cost critics are
computed with the acquired surfel properties. Note that the
following cost critics are calculated for each surfel in the surfel
set SO .
ts = max(| arctan(nsx , nsz )|, | arctan(nsy , nsz )|), (2)
where arctan(nsx , nsz ) and arctan(nsy , nsz ) define the roll
and pitch of the surfel respectively.
j
X APx + BPy + CPz + D
pds = 1/j | √ |, (3)
k=1
A2 + B 2 + C 2
The average point deviation from the surfel plane pds is
determined by the sum of all point distances to the surfel
plane divided by the number of points within the surfel.
hds = max(Pz1 , .., Pzj ) − min(Pz1 , ..., Pzj ), (4)
The height of a point is its value along the z-axis, with Pzj
being the z value of jth point. hds is the maximum height
difference among points of the surfel.
APx + BPy + CPz + D
gcs = ∀k ∈ {1, .., j}, max( √ ). (5)
A2 + B 2 + C 2
Where gcs is the distance of the farthest point from the surfel
plane (along the normal vector), the gcs should be less than
the distance of the robot chassis to the ground to ensure safe
ground clearance.
The individual critics are then linearly weighted by
Js = wt ts + wpd pds + whd hds + wgc gcs , (6)
where wt , wpd , whd and wgc are adjustable parameters to
weigh each critic according to the desired behavior. In the
default configuration, the values for each weight are set to
0.25, meaning they equally contribute to determining the
overall traversability cost of a region represented by a surfel.
Js becomes the cost associated with surfel Ss . Elevating each
surfel along their normal vector ns by de (see Eq. 7), we
achieve a thin layer that follows the structure of terrain with
a distance. de is configuration parameter that can be adjusted
according to physical properties of robot, distance from robot’s
center of gravity to the ground is a good default parameter for
the de . We refer to this elevated surfel set as SE .
v s = v s + de ns , (7)
At this stage, the elevated surfel set SE resembles a thin
layer (consisting of surfels) on top of the terrain underneath.
We add a volume to this thin layer by stacking several surfels
together (along with their normal vectors) with a step size
ssize . An illustration of this process is depicted in Fig. 2.
The result of this stacking looks like a “waffle” that can be
approximated to a cylinder if ssize is chosen small enough.
This volume is then used for planning and navigation on
unstructured terrains. We refer to this volume as elevation
state-space.
63

Fig. 2: Waffle surfels on top of an unstructured environment
patch. Stacked surfels that construct one waffle have identical
normal vectors, radius, and cost values. We achieve a con-
strained state-space suitable for unstructured terrain planning
and navigation with the side-by-side collection of surfel waf-
fles.
Fig. 3: Cost values are encoded with RGB channels to the
terrain point cloud map.
B. Integrating the Physical and Kinematic Constraints
A robot navigating in unstructured terrain is subject to the
limitations of maneuvers due to its physical and kinematic
constraints.
Physical constraints are robot-specific parameters. For in-
stance, the maximum tilt angle that a robot can bear while
navigating is a robot-specific physical limitation. Another
robot-specific limitation is ground clearance [9] which ensures
contact-free robot chassis-terrain interaction. We address phys-
ical constraints by regulating our cost critics (see Sec. III-A)
to a configurable range. The range for each cost critic is an
adjustable parameter and can be adapted according to the
robot type and desired behavior. For instance, setting a tight
range for cost critic ts (tilt of the terrain) from Sec. III-A
will lead to a more inclination-sensitive traversability map;
thus, the robot will not navigate with such settings through
highly inclined regions, see Fig. 9. In Fig. 9, the red-colored
patches are marked as non-traversable, resulting in cost critics
being out of defined ranges. Kinematic constraints, e.g., for
the Ackerman type of robot, are considered in the planning
stage. Typically, Dubins [1] or Reeds-Sheep [13] shortest
curves handle these constraints. When the planning problem
is aimed at unstructured terrains, the situation gets more
complicated as traditionally, Dubins and Reeds-Sheep are
computed on a 2D plane, but an additional dimension (z)
exists for unstructured terrains. Our method deals with this
situation by decomposing SE2 and z as separate components
(see Fig. 4) of elevation state-space. SE2 state-space is used
for robots operating in a flat environment with three degrees
of freedom (x, y, heading). Dubins or Reeds Sheep variants
64
Fig. 4: Elevation state-space is composed of SE2 and z
components. We account for the kinematic constraints of an
Ackerman type by considering Dubins or Reeds-Sheep space
for the SE2 element.
of SE2 members are used to account for kinematic constraints
related to the maneuvers. The constraint of the z component,
which enforces constant contact of robot base and terrain, is
handled by the elevated surfels introduced in Sec. III-A
C. Implementation Details
We implement the new state-space into the OMPL frame-
work. The implementation is done in the C++ programming
language. A few substantial C++ objects that construct the
body of our state-space implementation are Elevation State-
Space, Surfel-based Valid State Sampler (SVSS), and Surfel
Cost Optimization Objective (SCOO). We can distinguish
between traversable and non-traversable regions with the cost
regression explained in Sec. III-A. SVSS exploits this fact
and only generates samples within traversable areas. It is also
possible to enable discrete probability distribution to give a
higher probability of drawing samples from lower-cost regions.
The samples are drawn directly from the elevated surfel set
SE , in Eq. 8, we weigh the likelihood of a sample being drawn
according to their costs. The higher a surfel cost is, the less
likely SVSS will mark it as a sample for OMPL planners, the
probability is calculated with Equation 8.
P (i|(M/J0 ), (M/J1 ), ..., (M/Jn−1 )) =
Ji (8)
Pn−1 ,0 ≤ i < n
k=0 M/Jk
Where M is the max cost threshold, we assume 255. P
describes the probability of surfel Si being drawn from surfel
set SE as a sample, and Ji is the cost associated with surfel
Si .
Some OMPL planners, such as RRT* and PRM* [5] re-
ferred to as optimizing planners, can use an objective function
to optimize a path according to a given objective. The opti-
mizing planners use the objective function’s value to judge the
quality of the path to improve the quality of the path based on
this. SCOO sums up costs given in the Eq. 6, SCOO can be
used with other optimization objectives such as shortest length
obstacle clearance objectives already present in OMPL.
IV. E XPERIMENTS AND VALIDATION
We use data from a real environment consisting of unstruc-
tured terrain with various objects to evaluate our method. We
use a LIDAR-based SLAM approach to acquire the natural
environment’s point cloud map seen in Fig. 7. Recent SLAM
approaches [16], [17] and [6] can produce dense and accurate
point cloud maps using a LIDAR, IMU, and optionally GNSS.
LIO-SAM by Shan et al. [17] proved to produce reliable maps
of these methods. Therefore, we run our tests on the maps
produced by LIO-SAM. In addition to actual data from the real
world, we use point cloud maps generated from the Gazebo
simulator. As our robot platform, Thorvald II by Grimstad et
al. [3], Thorvald II is a modular agricultural robot suitable for
unstructured environment navigation.
A. Comparison to Standard ROS Navigation Framework
Two approaches, Mesh Nav [12] and Navigation2 [7], have
organized software implementations that we may compare our
approach as baselines. Navigation2 is a widely used navigation
framework that is built on top of a well-matured software
stack [8]. The primary use of this framework is in indoor
scenes; however, outdoor scenes are also possible. We realize
that, for even terrain patches, both our method and Navigation2
perform as expected. The performance difference reveals itself
when the terrain is uneven (e.g., steep inclinations). Our
approach correctly models the unstructured nature of outdoor
scenes, leading to superior performance over Navigation2. An
example case is depicted in Fig. 6 where a robot (Ackerman
type) is given an identical navigation task over an unstructured
terrain. Thanks to the terrain feature-aware nature of elevation
state-space, the planning, and navigation tasks are effortlessly
handled, whereas Navigation2 fails to address the tasks due
to a lack of accurate representation of uneven terrain since it
bases on a 2D occupancy grid.
B. Edge-Case Tests in Real Environment
The edge cases are aimed to qualitatively assess the planning
and navigation performance in challenging parts of an actual
environment map. In the first case, the robot must navigate
from various poses to one goal pose in the map depicted in
Fig. 7.
1) Navigating through an unstructured inclination: In this
case, the robot is expected to navigate through an unstructured
inclination; see Fig. 7. Although we assume a robot model
that can take on unstructured terrain, the robot is expected
to follow a smoother and less inclined path to minimize
energy consumption and risk factors. For this case, there are
approximately 2 meters of elevation variance from start to
goal, with the total distance to the goal being around 65 meters.
The planning and consecutive navigation for this case
consistently resulted in success. See Fig. 7 Fig. 5 for instance.
The planned paths are steered towards feasible regions, mainly
when the inclination raises. Fig. 7 and Fig. 5 belong to the
same scene, but experiments in Fig. 5 were performed during
winter.
2) Navigating through the excavation of the building site:
In this case, the robot tries to navigate through an excavation
residual of a building in the real environment. The challenge
of this case was the height difference of the excavation area
from the rest of the ground plane and its non-traversable
edges formed by piles of sand and gravel resulting from
some building works. This limits feasible regions for the safe
navigation of the robot. Repetitive planning and navigation
queries from the different start positions to the goal inside the
excavation resulted in successful navigation.
C. Comparison of Planners Performance in Elevation State-
space
The selected edge cases are insufficient to draw general
conclusions on planners’ performance in the proposed state-
space. We perform benchmarking for the success rates of
RRT* and PRM* planners from the OMPL on a randomly
generated problem set to draw a general conclusion on plan-
ners’ performance in the elevation state-space. The randomly
generated set consists of 100 planning problems in the real or
simulated map (randomly chosen for each problem) used in
the edge-case tests. All randomly generated problems satisfy
the following conditions:
• The Euclidean distance between the randomly generated
start and goal pose is from 40 meters up to 100 meters.
• There exists at least one exact solution for the generated
problem.
We verify that at least one solution exists for the generated
problem by setting the timeout parameter to a very long wait
time, 5 min. If the planner found an exact solution within that
time interval, we add the generated problem for benchmarking;
if the planner found no solution for the generated problem,
we skip it and generate another one until 100 problems are
available for benchmarking. A solution is considered exact if
the path has been connected to the goal state with less than 0.2
meters gap and approximate if this gap is less than 10 meters.
The success rate results from 100 runs are given in Fig. 8.
1) SCOO assessment: We introduced SCOO in Sec. III-C.
It is worth noting how this optimization objective affects the
behavior of planners. Planners tend to avoid higher-cost areas
with SCOO, including steep inclinations and rougher terrain
patches. In the figure, we compare results from two instances
of PRM* where one instance is configured with SCOO, and
the other instance is configured with the length optimization
objective. The length optimization objective tries to maintain
the shortest path possible. In contrast, the SCOO considers the
integrated cost values along this path, thanks to our state-space
representation’s available traversability measures(surfel costs).
In this case, the SCOO gets safer and less energy-consuming
paths. See an example in Fig. 9, where the SCOO steers the
path towards greener areas, indicating lower-cost zones.
D. Discussion
We provided results from randomly generated problems in
Sec. IV-C to evaluate the use and benefits of the proposed
method based on the robot model on hand. Point clouds
are accurate approximations of the actual 3D world and are
the most common output format from 3D SLAM methods.
Therefore, we argue that it is beneficial to base robot naviga-
tion directly on raw point clouds. We used the intermediate
surfel representation since we are interested in leveraging
65
Fig. 5: A sequence of images showing auto navigation of Thorvald II robot in a real unstructured snowy terrain with the help
of elevation state-space. In sequences, at the top-left third-person view of the robot, at the bottom-left robot’s POV, and on the
right side, the RVIZ view of the map is depicted. In the RVIZ view, the planned path also can be seen(yellow-colored line).

(a)

(b)
Fig. 6: The robot effortlessly goes through an inclined hill and reaches the desired goal in elevation state-space (a). In the
classical navigation framework, as the robot gets closer to the inclined terrain, it perceives the hill as an obstacle. While in
reality, it is traversable for the given robot model. This leads to confusion on the planner and controller, ultimately causing
navigation failure (b).

the abstract features of point cloud surfaces. Concepts that
proposed method introduced, i.e., SVSS and SCOO proved to
compliment the performance of the OMPL planners in the
context of unstructured terrain planning see Fig. 8.
With the existing state-space implementations in OMPL,
such as SE3 SE2, it was impossible to achieve feasible path
plans in unstructured terrains. The simple reason for this is
that these state-spaces are not aware of constraints(kinematic
or physical) that the planner must respect. In contrast, these
constraints are considered and respected in the proposed
elevation state-space. With the introduced cost critics, we can
readjust the behaviors of planners according to the desired
result. For instance, increasing the weight of average point
deviation from the surfel plane critic (pds in Sec. III-A) forced
plans to be generated in smoother areas rather than short-
cutting towards the goal. In unstructured terrains, defining the
characteristics of an optimal path is challenging. For instance,
which path is the most optimal and desired path with minor
energy consumption, the shortest length, or the least risk of
rollover?. We argue that the answer is application-specific.
Our method’s concept of incorporating traversability costs to
planning through SCOO becomes helpful in defining what
should be the characteristics of an optimal path because it
is possible to weigh the cost critics.
V. C ONCLUSION
This paper used the surfel representation of underlying
point clouds to introduce a constraint-aware state-space. The
outcomes of the experiments show that the method can be
used to navigate in difficult, uneven surroundings. Within
the suggested method, it is also possible to account for the
kinematic and physical constraints of different robot kinds.
The inefficiency of our approach when dealing with kinematic
constraints is one of its flaws. When employing Dubins or
Reeds Sheep state-spaces, it takes a long time for planners
to converge to near-optimal paths. In the future, we’d like to
expand on this work by looking at efficiency difficulties while
accounting for kinematic constraints and standardizing motion
planning and navigation for unstructured terrains using point
clouds.

66

Fig. 7: Example result of navigating through an unstructured
inclination mentioned in Sec. IV-B1. The robot start position
is marked with white circle and the white circle with the flag
is the goal position, in the upper side, a drone image of the
case is presented. At the bottom, a cost regressed map of the
environment and produced path (in cyan) are visualized.
Fig. 8: Success rates from OMPL planners for randomly
generated 100 planning problems on an unstructured terrain.
Without exploiting surface information of elevation, planners
suffer as most sampled states are generated in undesired areas.
However, guided by SVSS, the first instance of PRM* solves
all planning problems.
R EFERENCES
[1] L. E. Dubins. On curves of minimal length with a constraint on average
curvature, and with prescribed initial and terminal positions and tangents.
American Journal of Mathematics, 79(3):497–516, 1957.
[2] D. D. Fan, K. Otsu, Y. Kubo, A. Dixit, J. Burdick, and A. Agha-
Mohammadi. STEP: stochastic traversability evaluation and planning
for safe off-road navigation. CoRR, abs/2103.02828, 2021.
[3] L. Grimstad and P. J. From. Thorvald ii - a modular and re-configurable
agricultural robot. IFAC-PapersOnLine, 50(1):4588–4593, 2017. 20th
IFAC World Congress.
[4] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard.
OctoMap: An efficient probabilistic 3D mapping framework based on
octrees. Autonomous Robots, 2013. Software available at http://octomap.
github.com.
[5] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal
motion planning. CoRR, abs/1105.1186, 2011.
[6] S.-W. Lee, C.-M. Hsu, M.-C. Lee, Y.-T. Fu, F. Atas, and A. Tsai. Fast
point cloud feature extraction for real-time slam. In 2019 International
Automatic Control Conference (CACS), pages 1–6, 2019.
Fig. 9: Compared to the path length optimization objective,
SCOO maintains approximately the same path length with
lower traversability cost overall, improving the safety and
energy consumption during navigation. Refer to Fig. 3 for the
relation between RGB color and cost values. SCOO can also be
used with other optimization objectives to achieve composite
behavior.
[7] S. Macenski, F. Martin, R. White, and J. Ginés Clavero. The marathon
2: A navigation system. In 2020 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS), 2020.
[8] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, and K. Konolige.
The office marathon: Robust navigation in an indoor office environment.
In International Conference on Robotics and Automation, 2010.
[9] P. Papadakis. Terrain traversability analysis methods for unmanned
ground vehicles: A survey. Engineering Applications of Artificial
Intelligence, 26(4):1373–1385, 2013.
[10] H. Pfister, M. Zwicker, J. Baar, and M. Gross. Surfels: Surface
elements as rendering primitives. Proceedings of the ACM SIGGRAPH
Conference on Computer Graphics, 05 2000.
[11] A. Pfrunder, P. V. K. Borges, A. R. Romero, G. Catt, and A. Elfes.
Real-time autonomous ground vehicle navigation in heterogeneous en-
vironments using a 3d lidar. In 2017 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS), pages 2601–2608, 2017.
[12] S. Pütz, T. Wiemann, J. Sprickerhof, and J. Hertzberg. 3d navi-
gation mesh generation for path planning in uneven terrain. IFAC-
PapersOnLine, 49(15):212–217, 2016. 9th IFAC Symposium on In-
telligent Autonomous Vehicles IAV 2016.
[13] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both
forwards and backwards. Pacific Journal of Mathematics, 145(2):367 –
393, 1990.
[14] F. Ruetz, E. Hernández, M. Pfeiffer, H. Oleynikova, M. Cox, T. Lowe,
and P. Borges. Ovpc mesh: 3d free-space representation for local ground
vehicle navigation. In 2019 International Conference on Robotics and
Automation (ICRA), pages 8648–8654, 2019.
[15] R. B. Rusu and S. Cousins. 3d is here: Point cloud library (pcl). In 2011
IEEE International Conference on Robotics and Automation, pages 1–4,
2011.
[16] T. Shan and B. Englot. Lego-loam: Lightweight and ground-optimized
lidar odometry and mapping on variable terrain. In IEEE/RSJ Inter-
national Conference on Intelligent Robots and Systems (IROS), pages
4758–4765. IEEE, 2018.
[17] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela.
Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and
mapping. In IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), pages 5135–5142. IEEE, 2020.
[18] I. A. Sucan, M. Moll, and L. E. Kavraki. The open motion planning
library. IEEE Robotics Automation Magazine, 19(4):72–82, 2012.
[19] C. Wang, L. Meng, S. She, I. M. Mitchell, T. Li, F. Tung, W. Wan, M. Q.-
H. Meng, and C. W. de Silva. Autonomous mobile robot navigation
in uneven and unstructured indoor environments. In 2017 IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS),
pages 109–116, 2017.
[20] M. Wermelinger, P. Fankhauser, R. Diethelm, P. Krüsi, R. Siegwart, and
M. Hutter. Navigation planning for legged robots in challenging terrain.
pages 1184–1189, 10 2016.
67
Paper III

Navigating in 3D Uneven
Environments Through
Supervoxels and Nonlinear MPC

Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

2023 European Conference on Mobile Robots (ECMR)

III

69
 Navigating in 3D Uneven Environments through Supervoxels and
Nonlinear MPC
Fetullah Atas 1 Grzegorz Cielniak
Abstract—Navigating uneven and rough terrains presents diffi-
culties, including stability, traversability, sensing, and robustness,
making autonomous navigation in these terrains a challenging
task. This study introduces a new approach for mobile robots
to navigate uneven terrains. The method uses a compact graph
of traversable regions on point cloud maps, created through
the utilization of supervoxel representation of point clouds. By
using this supervoxel graph, the method navigates the robot
to any reachable goal pose by utilizing a navigation function
and Nonlinear Model Predictive Controller (NMPC). The NMPC
ensures kinodynamically feasible and collision-free motion plans,
while the supervoxel-based geometric planning generates near-
optimal plans by exploiting the terrain information. We con-
ducted extensive navigation experiments in real and simulated 3D
uneven terrains and found that the approach performs reliably.
Additionally, we compared resulting motion plans to some state-
of-the-art sampling-based motion planners in which our method
outperformed them in terms of execution time and resulting
path lengths. The method can also be adapted to meet specific
behavior, like the shortest route or the path with the least slope
route. The source code is available in a GitHub repository. 1 .
Index Terms—Uneven Terrain Navigation, Outdoor Robotics,
Motion Planning.
I. I NTRODUCTION
The use of robots in large outdoor environments with
uneven terrain has become increasingly popular, with appli-
cations ranging from delivery robots to legged robots nav-
igating unstructured terrains and mobile robots performing
agricultural tasks. While autonomous navigation in 2D indoor
environments has been well-established, the navigation of
outdoor uneven terrains poses various challenges that require
further investigation. These challenges include the need for
robust terrain traversability analysis to prevent stability issues
such as tipping over, efficient motion planning that avoids
collisions, and adaptability to dynamic environments. An
additional challenge is that different robot platforms have
different traversability capabilities, depending on their design
and locomotion mechanisms. Thus, developing a unified and
abstract navigation strategy for uneven environments that
meets the requirements of different robot models without
significant architectural or code modifications to the navigation
framework is a nontrivial task.
To address the challenges mentioned above regarding un-
even terrain navigation, this paper proposes a compact ap-
1 Norwegian University of Life Sciences {fetullah.atas,
lars.grimstad} @nmbu.no
2 University of Lincoln gcielniak@lincoln.ac.uk
1 https://github.com/NMBURobotics/vox nav
979-8-3503-0704-7/23/$31.00 ©2023 IEEE
2 Lars Grimstad 1
Fig. 1: The figure illustrates the Thorvald II robot’s capability to
operate in various rough uneven terrains. To fully utilize the robot’s
strengths in these terrains, it is crucial to have a reliable navigation
strategy.
proach using pre-built or online point cloud maps. The ap-
proach first identifies traversable regions based on the geomet-
rical features of the point cloud by constructing a supervoxel
representation of the point cloud, with this, the approach
determines the robot’s geometric motion plan towards a de-
sired reachable goal. The geometric plan is then executed
by a Nonlinear Model Predictive Controller (NMPC) that
adjusts the robot’s actions based on real-time feedback and
environmental changes (e.g. dynamic obstacles).
A. Related Work
1) General 3D Navigation: Several works for 3D navi-
gation have been proposed based on 2D processing such as
in Wang et al. [27] and Pütz et al. [21]. However, these
methods are limited as they do not exclusively consider rapid
elevation changes as shown in Atas et al. [2]. Pütz et al.
[20] proposed a mesh-based navigation where they created
triangulation of underlying point cloud. The idea is spiritually
similar to what we propose, however, our approach works
as anytime, meaning that the method calculates supervoxels
for each new navigation request while still meeting real-time
constraints as supervoxel construction is efficient. More recent
work by Fan et al. [4] uses e Conditional Value-at-Risk (CVaR)
based terrain traversability analysis to navigate in uneven
terrains. Additionally, Jian et al. [8] proposes a plane-fitting-
based navigation framework, PUTN for uneven terrains. Our
work is similar to that of PUTN, a major difference is that
for the global geometric path generation they propose plane-
fitting-based RRT*, however, the proposed motion planning al-
gorithm is not compared to more recent work in the sampling-
based planners that are highlighted in the next subsection of
related work. Additionally, Atas et al. [2] proposed a surfel-
71
based framework for uneven terrain navigation; however, the
work did not deal with dynamic obstacles and assumed static
environments.
2) Geometric Motion Planning: Sampling-based motion
planning has been demonstrated as an effective strategy to deal
with planning for robots in uneven environments in various
works such as in Jian et al. [8] and Atas et al. [2]. In the
following, we provide a brief review on sampling-based mo-
tion planning. The PRM Kavraki et al. [10] and RRT Lavalle
and Kuffner [12] algorithms and their dozens of variants
have paved the way for sampling-based motion planning. The
authors in Karaman and Frazzoli [9] investigated sampling-
based planners’ completeness and optimality properties to
understand formal guarantees further. Some sampling-based
planners were later improved to account for non-holonomic
constraints Palmieri et al. [18]. A library called Open Motion
Planning Library (OMPL) Sucan et al. [26] contains imple-
mentations of many sampling-based planners. The majority
of these planners are based on RRT* and PRM* Karaman
and Frazzoli [9] planners (e.g., RRTX Otte and Frazzoli [17],
LazyPRM* Bohlin and Kavraki [3], InformedRRT* Gammell
et al. [5], and so on), but a newer collection of planners
uses a structure that contains both graphs and trees (e.g.,
BIT* Gammell et al. [6], ABIT* Strub and Gammell [24],
AIT* Strub and Gammell [25]). Some methods utilize par-
allelized approaches (e.g., CFOREST Otte and Correll [16],
AnytimePartShortening (APS) Luna et al. [13]), in which
many planners execute concurrently on different threads while
planners inform each other of milestones reached, leading to
better performance overall.
3) NMPC for Collision-Free Control: Numerous studies
have established the effectiveness of Nonlinear Model Predic-
tive Control (NMPC) in achieving optimal control of diverse
robotic systems including mobile robots Salimi Lafmejani and
Berman [22], unmanned aerial vehicles (UAVs) Mansouri et al.
[15], and autonomous vehicles Yu et al. [28]. These systems
share a common trait, which is the presence of multiple
constraints that must be accounted for in each control cycle.
These constraints may include limitations on state dynamics,
non-collision requirements, and limits on state variables such
as velocity or acceleration, among others. The approach we
propose shares similarities with the work of Yu et al. [28],
who also present an NMPC scheme for obstacle-aware uneven
terrain navigation. However, our method offers two distinct
advantages over their work. Firstly, we validate our approach
through experiments on an actual robot, in addition to simu-
lations. Secondly, our method demonstrates a faster run cycle
of approximately 20Hz, whereas their reported control loop
runs at approximately 3Hz. The NMPC scheme proposed in
PUTN Jian et al. [8] also holds relevance. However, the authors
of this scheme have used a 2D kinematic model and the speed
of the control loop has not been explicitly reported.
B. Contributions
The specific contributions of our work include the follow-
ing:
72
• We introduce a new navigation strategy that utilizes
supervoxels and NMPC, which have been shown to be
effective in real uneven terrain navigation.
• Through a series of experiments in simulated and real-
world uneven terrains, we demonstrate that the proposed
method’s planning module outperforms existing state-of-
the-art sampling-based planners in terms of efficiency and
flexibility.
• We provide an open-source implementation of our ap-
proach.
II. A PPROACH
A. Problem Statement
A successful navigation task will produce a discrete motion
plan defined by state s̃ and control ũ sets at the final stage t.
The state and control sets are as follows:
s̃ = (s1 , s2 , ..., st ), ũ = (u1 , u2 , ..., ut ). (1)
Based on Eq. 1, we define the following components for
feasible navigation:
1) A finite state space X.
2) A control space U (s) for each state s ∈ X.
3) A state transition function f that produces next state
f (s, u) ∈ X.
4) A set of stages denoted by t that begins at t = 0 and
continues indefinitely, and a goal set XG ∈ X.
A successful navigation task is achieved by the function Υ
that maps every state to control; X −→ U .
B. General Overview
Our approach is divided into three stages. First, we develop
a method for evaluating the traversability of a point cloud
map by analyzing the geometric characteristics of local ter-
rain patches. The second stage involves the construction of
supervoxels on point cloud maps with quantified traversability
values, the supervoxels are then used to calculate minimal cost
paths either with the Dijkstra or A* graph search algorithms.
The Final stage utilizes the calculated geometrical path to
determine the optimal and kinodynamically feasible control
commands until a termination condition is met (e.g. goal
reached, collision detected, etc.). In the following sub-sections,
we will delve into the specifics of each stage of our approach.
C. Traversability Assessment via Local Geometric Features
Suppose that an environment is represented with a point
cloud map. We discretize this point cloud map through uniform
sampling with a voxel size of ds , resulting in a subset of
j points referred to as the sampled point set PS . From
this sampled point set, we extract local terrain features such
as roughness, tilt, and maximum height by analyzing the
geometrical relationship between the sampled points and their
neighboring points from the original point cloud map.
To obtain geometric features, we search for the radial
nearest neighbors of each sampled point in the original point
Fig. 2: A comprehensive overview of our approach is presented. The first step involves the calculation of the traversability of a given point
cloud map, taking into account the specific capabilities of the robot (e.g. max tilt angle, min distance for ground clearance, etc.). Secondly,
utilizing the obtained traversability map, the traversable regions are segmented into compact supervoxels. Finally, the NMPC optimization
process is employed for path tracking by producing optimal control values that are kinodynamically feasible.

cloud, which forms disk-like structures. Points within each

disk are then used to extract several geometric features.
The normal vectors for these disk-like structures are deter-
mined by fitting a plane to its points using RANSAC plane Fig. 3: Cost values are encoded with RGB channels to the terrain
fitting. This plane fitting is expressed in Eq. 2. point cloud maps.

Ax + By + Cz + D = 0,
(2)
ns = (nsx , nsy , nsz ) = (A, B, C)
The coefficients A, B, C, D represent the scalar equation of
a plane. The normal vector for this plane (of a disk) is given
by ns .
We use the following cost critics to assign a traversability
cost to each disk.
• Tilt of the slope within the disk, represented by td .
• Average deviation of points from the plane of the disk,
represented by pdd , to measure roughness. Fig. 4: The impact of varying parameters on the traversability map
• Maximum height difference of points within the disk,
is shown. In (a), (b), and (c), the effect of using different disk
radii is depicted. In (d), (e), and (f), the effect of choosing different
represented by hdd . max ranges for the tilt cost is depicted. Just as the tilt cost critic
• Ground clearance for the bottom chassis of the robot, can be adjusted, the range and weight of other cost critics can
represented by gcd . also be customized, providing flexibility to accommodate robots
These criteria aim to evaluate the terrain patches based on the with different traversability capabilities. The RGB values encode
traversability cost values. See Fig. 3.
robot’s physical limitations, such as the maximum tilt (roll or
pitch) angle, the roughness of the terrain, ground clearance,
etc. The resulting cost values will reflect these constraints and here, arctan(ndx , ndz ) and arctan(ndy , ndz ) represent the roll
indicate the disk’s suitability for traversal. The cost values for and pitch of the disk, respectively.
each disk are computed using the following equations, which Xn
APx + BPy + CPz + D
are based on the geometrical properties of the points within pdd = 1/n | √ | (4)
disks. The cost values are then distributed to all points in the k=1
A2 + B 2 + C 2
global point cloud map.
where n denotes the number of points within the disk. The
roughness of a disk is determined by the average point
td = arg max(| arctan(ndx , ndz )|, | arctan(ndy , ndz )|) (3) deviation from the disk plane, pdd . This value is calculated

73
by summing up the distances from each point within the disk
to the disk plane and then dividing the result by the number
of points in the disk.
hdd = arg max(Pz1 , .., Pzn ) − arg min(Pz1 , ..., Pzn ), (5)
0:n 0:n

The max height difference hdd is computed as the difference

between the highest and lowest points in the disk.

APkx + BPky + CPkz + D

gcd = ∀k ∈ {1, .., n}, max( √ ).
A2 + B 2 + C 2
(6) Fig. 5: Top view of path tracking over point cloud supervoxels. The
Ground clearance of each disk, gcs , is calculated as the local reference path, which is used as input for the NMPC algorithm,
maximum distance from the disk plane to any point within is composed of linearly interpolated path segments, with a time step
of T between consecutive states in the path.
the disk, along the normal vector direction. This value must
be less than the distance from the bottom of the robot chassis
to the ground to ensure safe navigation. We combine the value In Fig. 4, red points on the left picture are designated as
of each critic with their corresponding weight factors αt , αpd , non-traversable regions, and no supervoxels are created on
αhd , αgc to produce the final traversability cost value cs for these regions. Given the connectivity graph of supervoxels,
each disk: it is straightforward to compute cost-optimal paths over state
space X with graph traversal algorithms such as Dijkstra or
cs = (αt · td + αpd · pdd + αhd · hdd + αgc · gcd ) (7) A*. The area covered by a supervoxel is controlled with two
where αt , αpd , αhd , αgc are the weighting factors that deter- parameters, seed resolution psr and resolution pr . These two
mine the relative importance of each cost critic in the final parameters determine the resolution of the resulting geometric
traversability cost value. The weighting factors are adjustable path.
parameters to weigh each critic according to the desired
E. NMPC for Optimal Control Policy
behavior. Refer to Fig. 4 to see the impact of some cost critics
and other parameters (e.g. disk radius) on the traversability In this study, we employ the NMPC-based method to track
map. trajectory while ensuring compliance with dynamic constraints
and addressing potential collisions with stationary or moving
D. Supervoxels for Connected Safe Regions
obstacles that are represented with 3D ellipsoids. It is impor-
Our approach explores supervoxel phenomena of point tant to note that detecting obstacles is not a focus of this study.
clouds as a way of marking reachable regions for navigation The state dynamics are represented with a typical discrete
over uneven 3D terrains. dynamic system;
Supervoxels were proposed as an intermediate representa-
tion for a dense underlying point cloud to lower the compu-
sk+1 = f (sk , uk ), sk ∈ X, uk ∈ U (8)
tational requirements by segmentation algorithms, which are
analogous to superpixels for image segmentation. Supervoxels where the state vector is sk = [xk , yk , zk , ψk , θk , ϕk , v].
are suitable for navigation as they provide a connectivity graph This includes the robot’s x, y, z position, the robot’s yaw,
over the underlying point clouds. It is ensured that the super- pitch, and roll denoted by ψk , θk , ϕk , and finally v as the
voxels are evenly distributed within the actual observed space robot’s linear velocity along x − axis of its local frame. The
rather than being confined to the projected image plane. This control vector, uk = [v˙k , ψ˙k ], includes linear acceleration v̇,
is achieved through the implementation of a seeding method- and angular velocity ψ̇. The continuous kinematic model is
ology based in 3D space and a flow-constrained local iterative described by the following equation:
clustering algorithm that incorporates both color and geometric
     
features. Furthermore, the supervoxels can be utilized directly ẋ cos(ψ)cos(θ) 0 0
on raw point clouds that are generated by combining multiple  ẏ  sin(ψ)cos(θ) 0 0
     
calibrated RGB+D cameras or LIDARs, thereby providing  ż   cos(θ)  0 0  
      v̇
a full 3-dimensional supervoxel graph, which is capable of ψ̇  = v  0  + 0 1 (9)
      ψ̇
meeting the demands of robotic applications in terms of speed.  δ̇   0  0 0
     
We refer you to Papon et al. [19] for further details on  ϕ̇   0  0 0
supervoxel construction for point cloud segmentation. v̇ 0 1 0
In the previous subsection, a traversability measure was
regressed across the point clouds, allowing reachable por- The Fig. 5 illustrates the path tracking over the terrain. The
tions (non-red areas) of the point cloud to be derived. The goal of the NMPC strategy is to keep the system as close as
traversability of each local terrain patch was quantified by possible to the reference path while ensuring kinematic and
regressing the cost values to each patch. dynamic constraints.

74
 The NMPC optimization is formulated as;
u∗0:h−1 = arg min(Jt (s0:h , u0:h−1 ) + Jir (u0:h−1 )
0:h−1 (10)
+Jobs (s0:h , O))
subject to,
sk+1 = f (sk , uk ),
B(sk ) ∩ O = ∅, (11)
Umin ≤ U0:h−1 ≤ Umax
• The cost term Jt (s0:h , u0:h−1 ) is used to ensure that the
system follows the global path as closely as possible to
the Euclidean path between supervoxels. This cost term
also guarantees that the system’s kinematic constraints are
met since sk+1 = f (sk , uk ) is a hard equality constraint. Fig. 6: The figure presents the sample navigation scenes in a
• The term Jir (u0:h−1 ) represents the input rate cost, which simulated environment with uneven terrain and obstacles. The starting
aims to minimize the jerky movements of the control position of the robot is indicated by a checkered flag.
outputs.
• The term Jobs (s0:h , O) takes into account the cost associ- III. E XPERIMENTS
ated with the proximity of the system to obstacles, which
are represented as time-varying ellipsoids. Collision-free We conduct experiments in 3D uneven terrains with varied
control policies are guaranteed with B(sk ) ∩ O = ∅. slopes and roughness in both simulated and real environments.
Buildings, poles, trees, and other items are examples of objects
Specifically, the tracking cost Jt and the input rate cost Jir
in the environment. The maps are around 300x300 meters
are defined as follows:
h−1
in size. The simulated environment consists of more steep
X hills and more significant variance in terms of inclination. Our
Jt (s0:h ) = (si − sref ref T
i )Q(si − si ) ,
method relies on point cloud maps for the occupancy informa-
i=0
h−1
(12) tion of environments. Hence we construct point cloud maps
X
(ui − udv dv T of real environments with a SLAM method named LIO-SAM
Jir (u0:h ) = i )R(ui − ui )
i=0 by Shan et al. [23]. For building maps in real environments,
Q and R diagonal matrices are used to penalize specific we use the Ouster OS1-64 LiDAR and MTi-30-2A8G4 Xsens
state errors and input jerks. IMU. In the simulation, we rely on the Gazebo simulator
Finally, the obstacle costs are defined as follows: and simulated sensors and extract point cloud maps from the
simulated environment through a software plugin Koenig and
h−1 n Howard [11]. The onboard localization was performed through
XX
Dobs (s0:h ) = (xi − xobs 2 obs 2 a combination of ICP-based LIDAR, wheel odometry, and
j ) /(aj /2.0 + r) ,
i=0 j=0 IMU. The autonomy system is run on a ZOTAC ZBOX with
h−1
XX n (13) an Intel Core i7 CPU and an Nvidia RTX 2060.
+ (yi − yjobs )2 /(bobs 2
j /2.0 + r) ,
For the robot platform, the Thorvald II modular robot was
i=0 j=0 used Grimstad and From [7]. The software specifically de-
Jobs (s0:h ) = e(1.0/Dobs (s0:h )) signed for Thorvald runs on an Intel NUC computer equipped
with a Core i7 CPU, and the communication between all
As shown in Eq. 13, the cost is inversely proportional to components is managed by ROS 2 Macenski et al. [14].
the distance between the robot and obstacles. The variable n In evaluating the motion plan generated, two metrics are
represents the number of obstacles in O, h is the time horizon considered: the length of the path and the computation time.
considered in the NMPC setup and r is the robot radius. Each Tab. I is created based on these two evaluation metrics.
obstacle Oj is represented by a vector [xobs obs obs obs
j , yj , aj , bj ],
which are used in Eq. 14 to define the ellipse, see Fig. 2. A. Qualitative Performance Evaluation
(xobs )2 (y obs )2 In Fig. 8a, the robot arrives at the destination successfully;
+ obs 2 = 1 (14)
(aobs )2 (b ) the path depicted in blue is the resultant feedback plan, the
We implement the NMPC using CasADi Andersson et al. start pose is marked with a checkered flag. Similarly, in Fig. 8a
[1], a software framework that specializes in nonlinear opti- the robot navigates from a similar start pose (as in Fig. 8b) but
mization and optimal control. Although the NMPC algorithm to a closer goal pose, resulting in a near-optimal path depicted
can handle an arbitrary number of obstacles, to improve the in blue color. For Fig. 8a and Fig. 8b, we refer to optimality
speed of the control loop, we limit the number of obstacles in sense of Euclidean distance. However, with the availability
considered by the algorithm to those that are within 20 meters of traversability costs (represented with RGB colors in point
of the robot’s current position. cloud maps, seeFig. 3) we can re-adjust the sense of optimality

75
to refer to the traversability cost in point cloud maps. Hence
the method can be configured such that it can navigate the
robot with one of the following properties:
• least inclined path.
• shortest path.
• least rough path.
In Fig. 7 (a), we configure the cost term such that it
accounts for traversability costs. The resulting motion plans
avoid higher-cost regions, resulting in an optimal motion plan
where less-inclined, less-rough terrain segments are preferred
rather than a short distance. Hence the resultant motion plans
tend to navigate through greener regions. This allows the robot
to navigate through inclined terrains safely by avoiding steep
hills and rough terrain patches.
B. Tuning the Method
The tuning of our algorithm is straightforward as it involves
adjusting only a small number of configuration parameters.
Specifically, two parameters, known as seed resolution (psr )
and resolution (pr ), are used in the construction of supervoxels.
The seed resolution (psr ) is responsible for identifying the
radius of nearest neighbors that are used to compute the
supervoxel boundaries, while the resolution (pr ) determines
the actual size of the supervoxel. Smaller values of pr result
in finer resolution of the global path, but we recommend using
values in the range of [0.1, 0.25] for large-scale outdoor robot
navigation. In our reported results, we set psr = 1.0 and
pr = 0.2. The general rule is that pr should be large enough
to cover a number of points.
Q = Diag(10, 10, 10, 0.1, 0, 0, 0.1),
(15)
R = Diag(10, 100)
In Eq. 12, we introduced tracking cost and input rate
gain matrices Q and R. In this paper, we do not propose a
methodological approach to tune these gain matrices optimally.
However, from the experimental observation, we realize that
the angular rate needs to be penalized higher as otherwise,
the robot oscillates. From the optimality perspective, it is best
to control the robot as close as possible to the coarse plan
since the optimal property is explicitly contained within graph
traversal by Dijkstra or A*. Therefore in Q, we penalize posi-
tional errors on x, y, z heavier than the controllable heading ψ
and velocity v. In the presented results, Q and R were chosen
as in Eq. 15.
We also provide customizable objective selection in our
software implementation to optimize ensuing feedback plans,
allowing us to achieve plans with the various characteristics
itemized in Subsec. III-A.
C. Comparison to Sampling-based planners
In this subsection, we evaluate the geometric plan produced
by our approach. As baselines, we choose some of the state-
of-the-art sampling-based planners. We compare the proposed
method to various planners such as AnytimePathShortening
(APS), CFOREST, AIT*, and others. We create a benchmark
76
of planning problems consisting of 20 unique planning prob-
lems in the map depicted atFig. 7. Each planner is requested
to solve 20 different planning problems five times, resulting
in a total of 100 runs. The results of 100 runs are presented
in Tab. I.
Our method produced shorter paths in significantly less
time. Optimal sampling-based planners require a timeout pa-
rameter where they use all allowed time to construct a valid
path with minimal cost, in this case, the shortest length. To
observe the behavior of sampling-based planners with different
time constraints, we set the timeout to 10 and 20 seconds
consecutively. The planners can improve their performance
due to the available time in the 20-second setup. To establish a
baseline, we run the APS planner for 60 seconds; our method
almost achieves APS’s performance in less than one second.
Based on the results in Tab. I, our technique distinguishes
itself from sampling-based planners by its short execution
time and deterministic nature. Compared to sampling-based
planners, our method has the lowest final plan length deviation
in both setups (10 and 20 seconds), indicating consistency in
the obtained results.
Planners Length Mean (20 sec.) Length Mean (10 sec.)
RRT* 65.2 ± 9.6 75.7 ± 13.9
PRM* 51.2 ± 2.2 67.0 ± 12.0
AIT* 52.0 ± 3.7 58.8 ± 8.3
CFOREST 51.6 ± 3.2 58.3 ± 6.9
APS 50.7 ± 1.8 54.1 ± 6.1
APS (60 sec.) 48.7 ± 0.5
Ours (0.65 sec.) 49.5 ± 1.3
TABLE I: Means of acquired path lengths with 10 sec—timeout
from 100 runs. A lower mean value indicates that planners achieve
better performance for both metrics (shorter path in a shorter time).
The results in Tab. I show that the proposed approach is
able to produce shorter paths in a significantly shorter amount
of time compared to other planners. Specifically, the approach
generated 8.56% shorter paths than the next best planner (APS)
while only taking 6.68% of the time consumed by APS. It
was also observed that the best-performing baseline planner
(APS) required more than 40 seconds to surpass the proposed
method. Additionally, it should be noted that the proposed
method is an anytime planner that re-computes supervoxels
efficiently for each planning request and the reported times
also include the time spent for supervoxel creation.
Our method is able to generate plans at a significantly
faster rate than sampling-based planners. This is because
our method leverages the underlying terrain information in
an active manner, whereas sampling-based planners do not
take advantage of this information by default. As a result,
generating a feasible plan for uneven terrain (e.g. no jump-
overs above buildings) for a ground robot in uneven terrain can
be a time-consuming process using sampling-based planners.
This highlights the efficiency of the proposed method.
IV. C ONCLUSION
In this paper, we presented a novel approach for navigating
uneven terrains based on geometric traversability analysis,
Fig. 7: A comparison of the performance of the proposed approach with an objective set to the inclination cost (a) and Euclidean distance
(b), to that of the APS planner (c). The APS planner provides a longer path than the proposed approach despite taking more than ten times
longer to compute.
that arise from the robot and uneven terrain interaction.
(a)
(b)
Fig. 8: Example optimal motion plans resulting from the proposed the
approach in a real uneven environment. The chequered flag indicates
the robot’s start pose, while the robot’s icon indicates the robot’s final
pose.
supervoxels, and NMPC. Our experimental results on a real
robot operating in a 3D environment demonstrate the method’s
efficiency. Specifically, our approach generated better-quality
geometric plans in less time than state-of-the-art sampling-
based planners on a benchmark. Additionally, the method
enables a robot to navigate uneven 3D terrains with various
objectives, such as finding the shortest or least-inclined path
while dynamically avoiding obstacles.
One potential limitation of our proposed method is that it
does not explicitly model uncertainty that may arise from the
interaction between the robot and the uneven terrain. This
is due to the lack of sensors on the robot platform that
can perceive terrain parameters, such as soil compaction and
slippage estimation. In future work, we plan to extend our
approach to address this limitation and handle uncertainties
R EFERENCES
[1] Joel A E Andersson, Joris Gillis, Greg Horn, James B
Rawlings, and Moritz Diehl. CasADi – A software
framework for nonlinear optimization and optimal con-
trol. Mathematical Programming Computation, 11(1):
1–36, 2019. doi: 10.1007/s12532-018-0139-4.
[2] Fetullah Atas, Grzegorz Cielniak, and Lars Grimstad.
Elevation state-space: Surfel-based navigation in uneven
environments for mobile robots. In 2022 IEEE/RSJ
International Conference on Intelligent Robots and Sys-
tems (IROS), pages 5715–5721, 2022. doi: 10.1109/
IROS47612.2022.9981647.
[3] Robert Bohlin and Lydia Kavraki. Path planning us-
ing lazy prm. volume 1, pages 521 – 528 vol.1, 02
2000. ISBN 0-7803-5886-4. doi: 10.1109/ROBOT.2000.
844107.
[4] David D. Fan, Kyohei Otsu, Yuki Kubo, Anushri Dixit,
Joel Burdick, and Ali-Akbar Agha-Mohammadi. STEP:
stochastic traversability evaluation and planning for safe
off-road navigation. CoRR, abs/2103.02828, 2021. URL
https://arxiv.org/abs/2103.02828.
[5] Jonathan D. Gammell, Siddhartha S. Srinivasa, and Tim-
othy D. Barfoot. Informed rrt*: Optimal incremental
path planning focused through an admissible ellipsoidal
heuristic. CoRR, abs/1404.2334, 2014. URL http://arxiv.
org/abs/1404.2334.
[6] Jonathan D Gammell, Timothy D Barfoot, and Sid-
dhartha S Srinivasa. Batch informed trees (bit*): In-
formed asymptotically optimal anytime search. The
International Journal of Robotics Research, 39(5):543–
567, 2020. doi: 10.1177/0278364919890396. URL
https://doi.org/10.1177/0278364919890396.
[7] Lars Grimstad and Pål Johan From. Thorvald ii
- a modular and re-configurable agricultural robot.
IFAC-PapersOnLine, 50(1):4588–4593, 2017. ISSN
2405-8963. doi: https://doi.org/10.1016/j.ifacol.2017.
08.1005. URL https://www.sciencedirect.com/science/
article/pii/S2405896317314830. 20th IFAC World
Congress.
[8] Zhuozhu Jian, Zihong Lu, Xiao Zhou, Bin Lan, Anxing
77
 Xiao, Xueqian Wang, and Bin Liang. Putn: A plane-
fitting based uneven terrain navigation framework, 2022.
[9] Sertac Karaman and Emilio Frazzoli. Sampling-
based algorithms for optimal motion planning. CoRR,
abs/1105.1186, 2011. URL http://arxiv.org/abs/1105.
1186.
[10] L.E. Kavraki, P. Svestka, J.-C. Latombe, and M.H. Over-
mars. Probabilistic roadmaps for path planning in high-
dimensional configuration spaces. IEEE Transactions on
Robotics and Automation, 12(4):566–580, 1996. doi:
10.1109/70.508439.
[11] N. Koenig and A. Howard. Design and use paradigms
for gazebo, an open-source multi-robot simulator. In
2004 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS) (IEEE Cat. No.04CH37566),
volume 3, pages 2149–2154 vol.3, 2004. doi: 10.1109/
IROS.2004.1389727.
[12] Steven Lavalle and James Kuffner. Rapidly-exploring
random trees: Progress and prospects. Algorithmic and
computational robotics: New directions, 01 2000.
[13] R. Luna, I. A. Şucan, M. Moll, and L. E. Kavraki. Any-
time solution optimization for sampling-based motion
planning. In 2013 IEEE International Conference on
Robotics and Automation, pages 5068–5074, 2013. doi:
10.1109/ICRA.2013.6631301.
[14] Steven Macenski, Tully Foote, Brian Gerkey, Chris
Lalancette, and William Woodall. Robot operating
system 2: Design, architecture, and uses in the wild.
Science Robotics, 7(66):eabm6074, 2022. doi: 10.1126/
scirobotics.abm6074. URL https://www.science.org/doi/
abs/10.1126/scirobotics.abm6074.
[15] Sina Sharif Mansouri, Christoforos Kanellakis, Björn
Lindqvist, Farhad Pourkamali-Anaraki, Ali-akbar Agha-
mohammadi, Joel Burdick, and George Nikolakopoulos.
A unified nmpc scheme for mavs navigation with 3d
collision avoidance under position uncertainty. IEEE
Robotics and Automation Letters, 5(4):5740–5747, 2020.
doi: 10.1109/LRA.2020.3010485.
[16] M. Otte and N. Correll. C-forest: Parallel shortest path
planning with superlinear speedup. IEEE Transactions
on Robotics, 29(3):798–806, 2013. doi: 10.1109/TRO.
2013.2240176.
[17] M. Otte and Emilio Frazzoli. Rrtx: Real-time motion
planning/replanning for environments with unpredictable
obstacles. In WAFR, 2014.
[18] Luigi Palmieri, Sven Koenig, and Kai O. Arras. Rrt-
based nonholonomic motion planning using any-angle
path biasing. In 2016 IEEE International Conference
on Robotics and Automation (ICRA), pages 2775–2781,
2016. doi: 10.1109/ICRA.2016.7487439.
[19] Jeremie Papon, Alexey Abramov, Markus Schoeler, and
Florentin Wörgötter. Voxel cloud connectivity segmen-
tation - supervoxels for point clouds. In 2013 IEEE
Conference on Computer Vision and Pattern Recognition,
pages 2027–2034, 2013. doi: 10.1109/CVPR.2013.264.
[20] Sebastian Pütz, Thomas Wiemann, Jochen Sprickerhof,
78
and Joachim Hertzberg. 3d navigation mesh gen-
eration for path planning in uneven terrain. IFAC-
PapersOnLine, 49(15):212–217, 2016. ISSN 2405-
8963. doi: https://doi.org/10.1016/j.ifacol.2016.07.
734. URL https://www.sciencedirect.com/science/article/
pii/S2405896316310102. 9th IFAC Symposium on Intel-
ligent Autonomous Vehicles IAV 2016.
[21] Sebastian Pütz, Jorge Santos Simón, and Joachim
Hertzberg. Move base flex a highly flexible navigation
framework for mobile robots. In 2018 IEEE/RSJ Inter-
national Conference on Intelligent Robots and Systems
(IROS), pages 3416–3421, 2018. doi: 10.1109/IROS.
2018.8593829.
[22] Amir Salimi Lafmejani and Spring Berman. Nonlin-
ear mpc for collision-free and deadlock-free navigation
of multiple nonholonomic mobile robots. Robotics
and Autonomous Systems, 141:103774, 2021. ISSN
0921-8890. doi: https://doi.org/10.1016/j.robot.2021.
103774. URL https://www.sciencedirect.com/science/
article/pii/S0921889021000592.
[23] Tixiao Shan, Brendan Englot, Drew Meyers, Wei Wang,
Carlo Ratti, and Rus Daniela. Lio-sam: Tightly-coupled
lidar inertial odometry via smoothing and mapping. In
IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), pages 5135–5142. IEEE, 2020.
[24] Marlin P. Strub and Jonathan D. Gammell. Advanced bit*
(abit*): Sampling-based planning with advanced graph-
search techniques. 2020 IEEE International Confer-
ence on Robotics and Automation (ICRA), May 2020.
doi: 10.1109/icra40945.2020.9196580. URL http://dx.
doi.org/10.1109/ICRA40945.2020.9196580.
[25] Marlin P Strub and Jonathan D Gammell. Adaptively
Informed Trees (AIT*): Fast asymptotically optimal path
planning through adaptive heuristics. In Proceedings
of the IEEE International Conference on Robotics and
Automation (ICRA), pages 3191–3198, 31 May – 31
August 2020. doi: 10.1109/ICRA40945.2020.9197338.
[26] I. A. Sucan, M. Moll, and L. E. Kavraki. The open
motion planning library. IEEE Robotics Automation
Magazine, 19(4):72–82, 2012. doi: 10.1109/MRA.2012.
2205651.
[27] Chaoqun Wang, Lili Meng, Sizhen She, Ian M. Mitchell,
Teng Li, Frederick Tung, Weiwei Wan, Max. Q.-H.
Meng, and Clarence W. de Silva. Autonomous mobile
robot navigation in uneven and unstructured indoor en-
vironments. In 2017 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS), pages 109–
116, 2017. doi: 10.1109/IROS.2017.8202145.
[28] Siyuan Yu, Congkai Shen, and Tulga Ersal. Nonlin-
ear model predictive planning and control for high-
speed autonomous vehicles on 3d terrains. IFAC-
PapersOnLine, 54(20):412–417, 2021. ISSN 2405-
8963. doi: https://doi.org/10.1016/j.ifacol.2021.11.
208. URL https://www.sciencedirect.com/science/article/
pii/S2405896321022527. Modeling, Estimation and Con-
trol Conference MECC 2021.
Paper IV

CostTrust: A Fast-Exploring,
Iteratively Expanding
Frontier-Based Kinodynamic
Motion Planner

Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

Modeling, Identification and Control (MIC) Journal. Submitted.

IV

79
Modeling, Identification and Control, Vol. 44, No. 4, 2023, pp. 1–14, ISSN 1890–1328

CostTrust: A Fast-Exploring, Iteratively Expanding

Frontier-Based Kinodynamic Motion Planner
Fetullah Atas 1 Grzegorz Cielniak 2 Lars Grimstad 1

1
Faculty of Science and Technology (REALTEK), Norwegian University of Life Sciences, N-7491 Aas, Norway.
E-mail: {fetullah.atas,lars.grimstad}@nmbu.no
2
Lincoln Centre for Autonomous Systems (LCAS), University of Lincoln, Lincoln, UK. E-mail: gciel-
niak@lincoln.ac.uk

Abstract

Sampling-based motion planning has recently experienced considerable advancements, particularly in the
domain of geometric motion planning for diverse robotic systems. Nonetheless, kinodynamic motion
planning, which additionally considers a robot’s kinematics and dynamics to generate a motion plan,
remains an open challenge, necessitating further research. Kinodynamic planning, inherently more complex
than geometric planning, mandates that the planner not only adhere to motion constraints but also account
for system dynamics, including limitations in velocity and acceleration.
Furthermore, kinodynamic planning often requires the navigation of extensive state and control spaces,
rendering the process both computationally demanding and time-consuming. To effectively tackle kino-
dynamic motion planning, our proposed approach introduces a dynamic balance between exploration and
exploitation, continuously adjusted throughout the execution. Our bi-directional and multi-threaded al-
gorithm is specifically tailored to fulfill the efficiency requisites of kinodynamic motion planning. Our
comprehensive benchmarks, conducted on an Ackermann-steered robot and a dynamic quadrotor, demon-
strate that our method notably outperforms state-of-the-art baselines in terms of solution rate percentage
and path cost.
To facilitate accessibility and further research within the community, we have made the implementa-
tion of our method available ∗. It is integrated with the Open Motion Planning Library (OMPL), a widely
utilized resource in the field, enhancing our approach’s practical applicability †

Keywords: Motion Planning, Kinodynamic Planning, Sampling-Based Motion Planning

1 Introduction have a trajectory from an initial to a goal state. This

process must adhere to motion constraints and avoid
Motion planning constitutes a fundamental aspect of obstacles. Broadly, motion planning is categorized into
robotics, focusing on developing algorithms and tech- geometric and kinodynamic planning, differentiated by
niques that enable a robot or an autonomous agent to the nature of the constraints involved. Geometric plan-
∗ https://github.com/NMBURobotics/ompl ning is primarily concerned with generating a collision-
† https://github.com/NMBURobotics/vox nav free path, disregarding the dynamic constraints of the

doi:10.4173/mic.2023.1.1 © 2023 Norwegian Society of Automatic Control

81
 Modeling, Identification and Control

robot. Conversely, kinodynamic planning delves into tion, demonstrating effective exploitation.
optimizing a robot’s motion by meticulously consider- Our research introduces several significant advance-
ing its kinematics and dynamics, such as limitations on ments in kinodynamic planning:
acceleration, velocity, and specific motion constraints.
• Development of a heuristic-like function that effec-
Kinodynamic planning finds its application in
tively balances rapid pre-solution exploration with
various domains, including robotic manipulation,
post-solution exploitation, enhancing solution re-
aerospace systems, unmanned aerial vehicles, and au-
finement—a capability not prevalent in existing
tonomous vehicles. The dynamics of these systems typ-
state-of-the-art planners.
ically involve constraints on velocity, acceleration, and
other parameters, which inherently limit their maneu- • Implementation of a multi-threaded, bi-directional
verability. This limitation presents a significant chal- tree search method, significantly surpassing the
lenge for kinodynamic planners, necessitating a keen capabilities of current state-of-the-art kinody-
focus on the system’s maneuverability during method namic planners in terms of efficiency.
development. Consequently, this elevates the complex-
ity and intricacy of kinodynamic planning problems. • Integration of our planner into the widely-used
Thanks to their efficiency, sampling-based methods Open Motion Planning Library (OMPL), enhanc-
have been the mainstay of kinodynamic planning. The ing its usability and practical application, and pro-
basic idea behind these methods is to explore the state vision of benchmarking code to facilitate further
space by generating many samples and then use the research and comparisons.
information gathered to guide the search for a feasi-
ble trajectory. These methods benefit systems with
high-dimensional state spaces and complex constraints,
2 Related Work
as they can efficiently explore the state space without For the last two decades, sampling-based motion plan-
needing an explicit representation of the entire state ning algorithms have gained popularity in robotic mo-
space. tion planning due to their effectiveness and flexibility in
Our method represents a progressive stride in kin- handling complex systems and environments. Among
odynamic motion planning, focusing on a strategy the most well-known and widely-used algorithms in
that harmoniously balances exploration and exploita- this category are the Probabilistic RoadMap (PRM)
tion. Recent advancements in geometric motion plan- algorithm Kavraki et al. (1996) and the Rapidly-
ning, exemplified by contributions such as ABIT* exploring Random Tree (RRT) algorithm Lavalle and
and BIT* Strub and Gammell (2020b); Gammell Kuffner (2000), along with a plethora of their vari-
et al. (2020), emphasize exploiting established solu- ants. To gain a deeper understanding of the optimal-
tions through informed sampling. This technique se- ity properties of sampling-based planners, the authors
lectively samples regions with the potential to enhance in Karaman and Frazzoli (2011) conducted a compre-
the current solution, incorporating these samples into hensive examination of their completeness and guar-
the underlying data structure, typically a tree or graph. antees. Several libraries and frameworks have been
However, adapting this strategy to kinodynamic mo- developed to facilitate the use, and implementation
tion planning presents unique challenges, primarily due of sampling-based motion planning algorithms, such
to the added complexity of motion constraints inher- as the Open Motion Planning Library (OMPL) Su-
ent in dynamic systems. For instance, establishing a can et al. (2012). This library contains a wide range
connection between two states in such systems is not of sampling-based planners, including RRT* Karaman
straightforward, as it requires a steering function ca- and Frazzoli (2011) and PRM Karaman and Fraz-
pable of solving a two-point boundary value problem zoli (2011), as well as more recent algorithms that
(BVP). Addressing these limitations, we introduce a incorporate both graph and tree structures, such as
tree-search methodology that leverages random for- BIT* Gammell et al. (2020), ABIT Strub and Gammell
ward propagation of system states while conforming (2020b) and AIT* Strub and Gammell (2020a). Addi-
to the kinematic and dynamic constraints of the sys- tionally, some planners utilize parallelized approaches,
tem. A notable advantage of our approach is its inde- such as CFOREST Otte and Correll (2013) and Any-
pendence from boundary value problems (BVPs) that timePartShortening (APS) Luna et al. (2013), which
necessitate solving complex differential equations for allow for concurrent execution of multiple planners on
diverse dynamic systems. Furthermore, our method different threads, resulting in improved performance
excels in rapid exploration, attributed to its selective overall.
mechanism for choosing frontiers. Simultaneously, it To extend the applicability of sampling-based plan-
can continuously refine and improve the current solu- ners to systems with dynamics (e.g., velocities, ac-

2
82
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”

Figure 1: A series of snapshots illustrating the progression of our planner’s execution is presented. In snapshot
a., we observe the initial phase where multiple trees start their bi-directional search to connect the
start and goal vertices. Upon finding an initial solution, as shown in snapshot c., the focus of the
trees shifts towards exploitation of the current solution. This phase involves continuous optimization
of the path. Snapshot e. displays the culmination of this process, showcasing the optimized motion
plan. Throughout this sequence, the planner demonstrates its capability to dynamically transition
from exploration to exploitation, effectively refining the path to achieve an optimal solution.

celerations, etc.), i.e., kinodynamic planning, sev-
eral variants of RRT algorithm have been proposed.
Two notable examples include the RRT-Extend algo-
rithm LaValle and James J. Kuffner (2001) and the Ex-
pansive Space Trees (EST) algorithm Hsu et al. (1997).
These methods focus on the forward propagation of
dynamics and aim to efficiently and evenly explore the
state space, regardless of obstacles. Additionally sev-
eral works extended on Karaman and Frazzoli (2011) to
benefit from asymptotic optimality, e.g. Karaman and
Frazzoli (2013); Perez et al. (2012); Xie et al. (2015).
However, these works relied on the availability of a
steering function. A steering function computes the
optimal path between two states in a scenario with
no obstacles. Implementing a steering function is of-
ten associated with solving a two-point boundary value
problem (BVP). This problem involves solving a dif-
ferential equation while adhering to specific boundary
conditions. It is widely acknowledged in the field that
this can be challenging, considering the complexity of
some systems (e.g., a quadrotor with 15 DOF). Re-
cently, there has been an intense effort to remove the
need for the steering function in the motion planning
community. In Li et al. (2014), a kinodynamic asymp-
totically optimal planner named as SST was proposed
based on a simplified random forward search tree that
does not rely on a steering function. A theoretical
analysis of the proposed algorithm was provided, which
showed that as long as an adequate number of Monte
Carlo propagations (random controls and duration) are
added to the search tree, the algorithm ensures asymp-
totic optimality. A meta-algorithm that produces an
asymptotically optimal kinodynamic planner was pre-
sented in Hauser and Zhou (2016), given any feasi-
ble kinodynamic planner as a subroutine without using
steering functions or numerical boundary-value prob-
lem solvers. The increase in the use of heuristics has
also become prevalent in kinodynamic planners Little-
field and Bekris (2018). Authors in Kleinbort et al.
(2018, 2019) have led to a deeper understanding of

3
83
 Modeling, Identification and Control
the properties of asymptotically optimal kinodynamic
planning with milder assumptions, which the proposed
method also takes advantage of. Most recently, authors
of Shome and Kavraki (2021) proposed a new approach
for kinodynamic motion planning using sampling tech-
niques to estimate the connectivity of high-dimensional
configuration spaces. The approach uses bundles of
kinodynamic edges to cover the state space before a
query arrives and is shown to be asymptotically opti-
mal and to find high-quality solutions quickly in exper-
imental validation. Upon analyzing the existing liter-
ature in sampling-based motion planning, a noticeable
trend emerges: the increasing adoption of heuristics in
the most effective geometric motion planners. Yet, this
trend has not been as prevalent in kinodynamic motion
planning, likely due to its added complexity. Our work
distinguishes itself from existing kinodynamic planners
by placing a significant emphasis on the use of heuris-
tics. This strategy is employed to rapidly explore the
extensive state space. Once an initial solution is found,
our approach shifts towards exploitation, which can
also be construed as a heuristic-driven strategy. This
dual focus on exploration followed by exploitation, un-
derpinned by heuristic principles, sets our approach
apart in kinodynamic motion planning.
3 Problem Setup
In the present work, we delve into the domain of kin-
odynamic motion planning, a detailed exposition of
which is provided in Sec. 4. Kinodynamic motion plan-
ning is a specialized subset of motion planning that
intricately incorporates the dynamics of a system into
the planning process. This includes considering con-
straints imposed by the system’s velocity and acceler-
ation, as well as its equations of motion while charting
a trajectory from an initial state to a designated goal
state. The primary objective of kinodynamic planning
is to identify a path that is not only feasible but also
optimal, adhering to both geometric and dynamic con-
straints.
We assume a d-dimensional smooth manifold for the
state space X. The goal region is assumed to be in
the free region of state space XF such that XF ⊂ X,
xgoal ∈ XF , δgoal > 0 and Xgoal = Bδgoal (xgoal ). δgoal
denotes obstacle clearance and Bδgoal (xgoal ) is ball (Eu-
clidean) centered at xgoal ∈ Rd with radius r. Since we
consider a dynamic system, a control space is denoted
with U ⊆ RD . The considered system evolves from the
current state with given control inputs in the following
manner;
˙ = f (x(t), u(t)), x(t) ∈ X, u(t) ∈ U
x(t) (1)
4
84
The system is assumed to be Lipschitz continu-
ous for both x and u such that; ∃Kx , Ku > 0,
∀x0 , x1 ∈ X, u0 , u1 ∈ U ;
||f (x0 , u0 ) − f (x0 , u1 )|| ≤ Ku ||u0 − u1 ||
(2)
||f (x0 , u0 ) − f (x0 , u1 )|| ≤ Kx ||x0 − x1 ||
Definition 1. A valid kinodynamic trajectory π is
produced by propagating system forwards (see Eq. 1)
starting from π(0) by applying control function Υ :
[0, tπ ] ⊂ U resulting in π : [0, tπ ] ⊂ F .
Definition 2. A piecewise constant control func-
tion Υ is composed of multiple constant control func-
tions Υi , each defined over a specific interval of time
[0, ∆t], where each constant control function Υi maps
to a specific control value ui that belongs to set U . The
number of constant control functions is represented by
a natural number k ∈ N .
In optimal kinodynamic motion planning, the goal is
to identify a control function Υ and a trajectory π that
lives in subset F . The planner should also minimize the
overall cost of the trajectory, which is calculated as the
integral of a cost function g(π(t), Υ(t)) over the entire
duration of the trajectory, from time 0 to tπ . More
compactly the cost of π, g is denoted as COST (π).
4 Approach
Our algorithm is designed to identify an optimal mo-
tion plan for a specified start-goal pose, utilizing ran-
dom forward propagation of the system’s states. To
achieve this, the algorithm accommodates input con-
straints of the system, such as limits on velocity and ac-
celeration. A prerequisite for the algorithm’s effective
operation is the prior knowledge of the system model,
which enables the calculation of subsequent system
states based on the current state and control inputs.
This approach often necessitates navigating extensive
state spaces, presenting a significant challenge to the
algorithm’s time efficiency. In our novel approach, the
algorithm concurrently grows 2N trees, with N trees
originating from the start pose and the remaining N
from the goal pose. Here, N represents the number of
threads, a configurable parameter in our method. This
design inherently renders our planner multi-threaded, a
distinction that, to the best of our knowledge, is unique
in this field of study. The rationale for allocating an
equal number of trees to both the start and goal poses
is to enhance the likelihood of discovering an initial
solution faster. This strategy addresses the inherently
stochastic nature of the process, stemming from the
non-deterministic control sampling, which significantly
influences the initial solution discovery. Upon finding
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”

Figure 2: On the left side, we visualize the exploratory phase of the algorithm. Before finding an initial solution,
the algorithm employs a predominantly explorative heuristic alongside multiple bi-directional trees.
On the right side, following the discovery of an initial solution, all trees adapt to focus on the current
solution. They aim to enhance the solution’s cost-effectiveness, guided by the direction of the tree
that first identified the initial solution.

the initial solution, the algorithm enters a phase of con-
tinuous exploitation and optimization of this solution.
This optimization process persists until a predefined
termination condition is satisfied. In the subsequent
subsections, we will elaborate on the specific subcom-
ponents of our planner, which we have named ’Cost-
Trust’.
4.1 Definitions
A tree structure is initiated from a root vertex and sys-
tematically extends outward. This expansion aims to
establish a linkage between the root and the designated
target vertex. Subsequently, we explain the concept of
a vertex, elaborating on its intrinsic properties and sig-
nificance.
Vertex in Motion Planning: In the context of
our motion planning algorithm, a vertex is conceptual-
ized through a structure encapsulating key attributes
essential to the planning process:
Our motion planning algorithm utilizes this struc-
tured vertex representation for the search space; see
Tab. 1. Furthermore, these vertices are organized
within a nearest-neighbor structure, facilitating effi-
cient query processing Sucan et al. (2012).
It is crucial to note that the terms ’root-target’ and
’start-goal’ are not used interchangeably in our ap-
proach. Our bi-directional strategy means that for
some trees, the ’target’ vertex may actually be the
’start’ vertex. Similarly, the ’root’ vertex could be syn-
onymous with the ’goal’ vertex in certain contexts. In
any given problem setup, while the ’start’ and ’goal’
vertices remain consistent, the trees, depending on
their direction, may treat either of these vertices as
the ’root’ or ’target’.
4.2 Selecting Frontier Vertexes
After initializing a tree with the root vertex, we cre-
ate branches by applying random control samples to
the root vertex’s states. As the algorithm progresses,
the vertex count escalates, making selecting vertices
for further expansion increasingly critical. Our objec-
tive is to identify and select vertices most beneficial to
rapid exploration initially. In this context, such ver-
tices are termed ’frontiers.’ Random control sampling
is applied to these frontiers, a process we will present
in the following subsection.
In Fig. 3, we illustrate the opposing trees utilized
in our approach. The criteria for selecting frontier
vertices are based on several key properties: firstly,
the number of branches in a vertex nb ; secondly, the

5
85
 Modeling, Identification and Control

Table 1: Vertex Attributes in Our Planner

Attribute Description
State The current state of the sys-
tem, capturing its position,
orientation, and other rele-
vant parameters.
Control The control input applied to
the system, influencing its
transition to the next state.
Cost A numerical value represent-
ing the cumulative cost in-
curred to reach this vertex
from the root.
Control Duration The time period for which a
specific control input is ap-
plied.
Blacklisted Status A flag indicating whether the Figure 3: The presented figure depicts the bi-
vertex is excluded from fur- directional tree expansion strategy, which
ther exploration due to fac- operates by simultaneously expanding two
tors like infeasibility or colli- distinct trees rooted at the start and goal
sion. to enhance the algorithm’s efficiency. The
Root Status A designation that identifies dashed paths represent the propagation of
whether the vertex serves as states forward with a given control input.
the starting point of a search
tree.
Solution Relevance A boolean status indicating for a predefined subset of frontiers, identified in the
whether the vertex is part initial phase. This subset is then re-sorted, taking into
of the path identified by the account the cumulative distance of each vertex to its
planner. K nearest neighbors. Following this re-sorting, ver-
Branches A collection of subsequent tices located in sparser neighborhoods are given higher
vertices originating from the priority. The two-step nature of the frontier selection
current vertex, representing process is fundamentally driven by considerations of
possible paths forward. efficiency. Calculating the nearest K vertices for every
Parent A reference to the preceding vertex in the tree becomes increasingly inefficient as the
vertex, forming the connec- tree expands. Therefore, the initial step serves as an
tion in the path or search tree. efficient preliminary process, identifying frontiers with
a higher likelihood of enhancing the algorithm’s ex-
ploratory capability. The subsequent step is designed
accumulative cost associated with a vertex c, calcu- to ensure efficiency by focusing on these pre-selected
lated from the root; and thirdly, the density of vertices frontiers, thereby streamlining the overall process.
within its immediate neighborhood d.
The selection process is conducted in two steps. 4.3 Frontier Expansion
First, we calculate an initial score, denoted by s, for
each vertex, based on the parameters: the number of As indicated previously, the frontiers designated for
branches (nb ) and the cumulative cost (c). tree expansion act as a heuristic. In the ensuing phase,
the algorithm expands these frontiers, employing ran-
s = 1/(nb + 0.001) + c (3) dom control inputs with random duration alongside a
random number of branches. As outlined in Tab. 1,
In the subsequent phase of the selection process, the control inputs and their durations are applied to the
vertices are arranged in descending order based on their states of these frontier vertices. Upon confirmation of
score, denoted as s, see Eq. 3. Vertices with higher being collision-free, the new vertices are integrated into
scores are typically those at the interface of explored the tree. In instances where all branches originating
and unexplored spaces. The second step involves con- from a particular frontier vertex do not progress due to
sidering the neighborhood density, represented as d, a collision, such a vertex is designated as ’blacklisted.’

6
86
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”

Figure 4: Initially, our algorithm initiates tree growth from both the start and goal vertices, as illustrated in
panels a., b., and c. Following discovering a solution, all trees and their respective threads shift focus
to exploit this initial solution. The objective is to enhance the solution, aligning with the direction
established by the tree that first identified the solution. For instance, as shown in panel f., a path
with a more favorable cost is achieved by strategically exploiting the current optimal solution. The
solution exploitation can run indefinitely.

Consequently, this status leads to its exclusion from
being selected in the subsequent iteration of frontier
identification.
After the successful expansion of a frontier, an ad-
ditional critical evaluation is conducted. This assess-
ment determines whether the newly added vertex can
be connected to the goal at a lower cost than that of
the existing solution if one exists. If this condition is
met, the trees are reset, and the current best solution
is updated accordingly.
Xs , while the blue tree originates from Xg , the goal
pose. It is important to note that, although these trees
are independent, the multi-threaded architecture of our
method enables their simultaneous growth, increasing
the efficiency dramatically as evidenced by findings in
Sec. 5. Upon the identification of an initial solution, all
trees are reinitialized to commence from the same root
vertex as the tree that initially identified the solution.
A detailed illustration of the planner’s progression is
provided in Fig. 4.

4.4 Initial Solution and Plan Optimization

Prior to the discovery of an initial solution, the trees,
which operate independently, undergo bi-directional
growth. Each tree is rooted either at the start or the
goal, as depicted in Fig. 3. The tree represented in
red color originates from the start pose, denoted as
The algorithm persistently exploits the current opti-
mal solution, intending to improve its cost. This persis-
tent refinement process underpins the asymptotic op-
timality of our approach. The stages of this ongoing
solution exploitation are visually detailed in d., e., and
f. of Fig. 4.

7
87
 Modeling, Identification and Control

4.5 Algorithms
In this subsection, we delve into the core algorithms of
our approach, accompanied by a detailed discussion of Algorithm 2 Select Explorative Frontiers
the corresponding pseudocode. Require: max number, nn structure
Ensure: Selected set of frontier nodes
Algorithm 1 Solve Planning Problem 1: f rontier nodes ← listNodes(nn structure)
Require: ptc {Planner Termination Condition} 2: Remove blacklisted nodes from f rontier nodes
Ensure: PlannerStatus ∈ 3: for each node ∈ f rontier nodes do
{EXACT, APPROXIMATE, INVALID} 4: score[node] ← computeScore(node)
1: Initialize start state and goal state 5: end for
2: if isValid(start state) = FALSE ∨ 6: Sort f rontier nodes based on score
isValid(goal state) = FALSE then 7: f rontier nodes ←
3: return INVALID clip(f rontier nodes, max number)
4: end if 8: for each node ∈ f rontier nodes do
5: for t = 1 to numThreads do 9: density[node] ← computeDensity(node)
6: direction ← t%2 = 0 ? start to goal : 10: end for
goal to start 11: Sort f rontier nodes based on density
7: CALL Algorithm 2 12: f rontier nodes ←
8: CALL Algorithm 3 clip(f rontier nodes, max number)
9: end for
10: Wait for all threads to complete
11: bestP ath ← findBestPath()
12: if bestP ath ̸= NULL then
13: Update bestControlP ath and controlT rees
14: VisualizePaths Algorithm 3 Expand Frontiers
15: PlannerStatus ← EXACT or APPROXIMATE Require: f rontier nodes, num branch to extend,
16: end if nn structure, ptc, target property, path,
17: Clear data structures control paths vertices, exact solution f ound,
18: return PlannerStatus should stop exploration, current best path
Ensure: Expanded set of frontier nodes
In the proposed multi-threaded control planning 1: for each node ∈ f rontier nodes do
framework, three core algorithms interact to refine the 2: for i = 1 to num branch to extend do
solution path in a kinodynamic planning problem it- 3: if ptc = TRUE then
eratively. The primary algorithm, Solve Planning 4: return
Problem (Algorithm 1), orchestrates the overall plan- 5: end if
ning process. It initializes the planning scenario, in- 6: (new state, valid) ←
cluding the start and goal states, and then iteratively propagate(node, randomControl())
invokes parallel threads. Each thread independently 7: if valid then
executes a cycle of explorative frontier selection and 8: vertex ← createVertex(new state)
expansion, managed by Algorithms 2 and 3, respec- 9: vertex.cost ←
tively. computeCost(node, new state)
Select Explorative Frontiers (Algorithm 2) plays 10: if isCloseToGoal(vertex, target property)
a critical role in guiding the search process. Within then
each thread cycle, this algorithm selects a subset of 11: path ← updatePath(vertex)
frontier nodes from the rapidly-exploring tree. The se- 12: end if
lection is based on a scoring mechanism that considers 13: Add vertex to nn structure
both the cost associated with each node and the num- 14: end if
ber of existing branches, thereby balancing exploration 15: end for
and exploitation. The algorithm prioritizes nodes that 16: end for
present a beneficial trade-off between these two fac- 17: EvaluateBranches(f rontier nodes,
tors, ensuring an efficient yet thorough exploration of 18: current best path, should stop exploration)
the search space.
Expand Frontiers (Algorithm 3) is then called to
expand the selected frontiers. This algorithm gener-

8
88
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”
ates new states by applying random controls to the
chosen frontier nodes, effectively growing the search
tree. It ensures that new states are valid and checks
for their proximity to the goal, updating the solution
path whenever a more cost-effective route is discovered.
Extending frontiers is pivotal in exploring unseen ar-
eas of the state space and getting closer to an optimal
solution.
Together, these algorithms enable a dynamic and
adaptive approach to kinodynamic planning. By utiliz-
ing multiple threads, the Solve Planning Problem
algorithm ensures diverse search directions and a ro-
bust state space exploration. The interplay between
Select Explorative Frontiers and Expand Fron-
tiers algorithms allows for a guided yet flexible search
strategy, continually adapting to the evolving topol-
ogy of the tree. This synergy effectively balances ex-
ploration and exploitation, significantly enhancing the
planner’s ability to find optimal or near-optimal solu-
tions for systems with complex kinodynamics.
5 Experimental Results
Our evaluation methodology encompassed two distinct
dynamic systems: an Ackermann-steered vehicle-like
robot and a dynamic quadrotor with 15 state dimen-
sions. Both systems included state and control com-
ponents, with the controls for each system being sam-
pled within a range specified by the user. To assess
the efficacy of our proposed method in addressing kin-
odynamic planning problems, we employed two maps
featuring a variety of objects. The map illustrated in
Fig. 2, for instance, spans an area of 18x6 meters.
Our study included all prominent kinodynamic plan-
ners available in the Open Motion Planning Library
(OMPL) to ensure a comprehensive evaluation. These
planners were compared against our proposed planner.
The ensuing sections detail the results, generated on a
laptop equipped with an Intel Core i7-6700HQ CPU,
operating at 2600 MHz with eight cores, and supple-
mented by 16GB of memory.
Our primary metric for evaluation is the resulting
path cost, quantified by the accumulative control du-
ration of the solutions generated by the planners. The
exact solution rate is also presented as a metric in the
benchmarks. Each planner is allowed an identical time
frame to produce its results. Given the larger state
and control dimensions of the Quadrotor compared to
the Ackerman robot, a different amount of time allo-
cation is employed for these two benchmarks. Specifi-
cally, planners are provided 10 seconds for the Acker-
man robot and 40 seconds for the Quadrotor.
A majority of the baseline planners, including our
own, fall under the category of optimizing planners.
This implies that these planners are designed to uti-
lize the entirety of their allotted time to enhance their
solutions, provided they achieve a solution within this
timeframe. To ensure a fair comparison, all planners
are assigned a fixed time duration for operation. How-
ever, due to the significantly larger state space encoun-
tered in dynamic quadrotor planning, these planners
are allocated four times the amount of time given for
planning with the Ackermann-steered robot. This ad-
justment acknowledges the increased complexity and
computational demands of the quadrotor’s state space.
5.1 Ackermann-steered vehicle-like robot
Figure 5: The figure illustrates a sample from our
benchmark results for kinodynamic motion
planning of an Ackermann-steered robot.
The motion plans generated by the various
contestant planners are depicted using dis-
tinct colors for clarity. Alongside each plan-
ner’s name, the corresponding path costs are
enumerated. Notably, our planner, Cost-
Trust, achieves the lowest cost path, which
is marked with a green color.
The Ackermann steering system, often used in vehicle-
like robots, is characterized by a unique geometry that
allows the front wheels to turn at different angles.
This system is ideal for ensuring that all wheels fol-
low their respective circular paths during a turn, min-
imizing slippage. The typical equations governing an
Ackermann-steered vehicle are:
xnext = x + ∆t · v · cos(ψ + β)
ynext = y + ∆t · v · sin(ψ + β)
v
ψnext = ψ + ∆t · · sin(β)
Lb (4)
vnext = v + ∆t · a
 
La
β = arctan · tan(df )
La + Lb
In these equations: ∆t is the time step. x, y, ψ, v
are the current state variables representing position,
9
89
 Modeling, Identification and Control

orientation, and velocity. df is the front wheel steer-

ing angle. a is the acceleration. La , Lb are distances
from the vehicle’s center of gravity to the front and
rear axles, respectively. β is the slip angle, which is
a function of the steering angle df and the vehicle’s
geometric parameters La , Lb .

The forward propagation of a vehicle-like robot sys-

tem states is described in Eq. 4. The system’s state
vector, Xc = [x, y, ψ, v], is subjected to a randomly
sampled control vector, Uc = [a, df ], for a randomly
selected duration in the interval [0, Tmax prop ], result-
ing in the system transitioning to next state Xn . This
process is repeated to expand the tree as outlined in
Subsec. 4.5. The environment used for this use-case is
(a)
a house-like setting, as depicted in Fig. 1.

The start and goal states are placed such that the
algorithm is forced to explore the state space since the
only connection point where the planners can connect
start-goal states is a gate at the bottom right of the
map. For the start-goal states depicted in Fig. 1 with (b)
3D axes, a benchmark is run 100 times. The results,
presented in Fig. 6, indicate that the proposed plan- Figure 6: In (a), we present the exact solve rate per-
ner’s solve rate for 100 problems is comparable to that centages for kinodynamic motion planning
of RRT*, both showing a solve rate of 89 % while of an Ackermann-steered vehicle-like robot.
outperforming all other baselines. However, the pro- In (b), we focus on the resulting path costs,
posed method’s efficiency lowers the resulting path cost taking into account only those paths derived
within the given time, depicted in the bottom image of from exact solutions. Our planner, Cost-
Fig. 6. Regarding the solve rate, the proposed planner Trust, demonstrates superior performance
failed to find a solution in 11 cases. In rare instances, over baseline planners in terms of path cost
the planner may struggle to establish a connection as efficiency. It achieves a comparable solve rate
the number of nodes in the tree increases, leading to to RRT*, registering an 89% success rate
longer execution times for each iteration. We believe in finding solutions while outperforming the
the cause of the missed cases could be attributed to the rest of the baselines.
dense tree structure utilized in the proposed planner.
Specifically, maintaining a dense tree structure may in-
crease the likelihood of prolonged exploration of some
narrow passages, potentially increasing the number of
missed cases. To address this issue, we have modi-
fied the implementation of our approach such that the
planner can be configured to maintain a sparse tree
structure similar to that of SST, which may reduce the
occurrence of missed cases.

10
90
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”

Figure 7: The figure showcases a representative sample

focusing on kinodynamic motion planning for
a quadrotor. Notably, as depicted in the fig-
ure, CostTrust surpasses existing methods in
its performance. It stands out as the sole ap-
proach that consistently finds the exact so-
lution. In contrast, the other baseline meth-
ods predominantly yield infeasible or approx-
imate solutions.

5.2 Quadrotor
Figure 8: In (a), we present the exact solve rate per-
Sampling-based motion planning demonstrates marked centages, and in (b), resulting path costs for
effectiveness in higher-dimensional state spaces, where kinodynamic motion planning of a quadro-
discretization-based methods become computationally tor. Our planner, CostTrust, surpasses other
infeasible due to exponential computation cost escala- planners significantly in terms of the number
tion. To showcase the efficacy of our kinodynamic mo- of exact solutions achieved.
tion planner, we extended our benchmarking to include
kinodynamic planning for a quadrotor characterized by
15 state dimensions and six control dimensions. Con-
trol sampling for the quadrotor was confined within T = m · (g + az + Kp · (zd − z) + Kd · (żd − ż)),
 
a user-defined range, conforming to the design limita- (ax · sin(ψ) − ay · cos(ψ))
τϕ = Kϕ · −ϕ ,
tions of quadrotors. The forward propagation model g
employed for the quadrotor is detailed in Eq. 5.  
(ax · cos(ψ) − ay · sin(ψ))
τθ = Kθ · −θ ,
In Eq. 5; The system’s state vector comprises 15 el- g
ements. The positions are denoted [x, y, z]. T is the τψ = Kψ · (ψd − ψ),
thrust. m is the mass. g is the gravitational accel-
eration. ax , ay , az are the accelerations in respective ϕ̇+ = τϕ · ∆t,
axes. Kp , Kd , Kϕ , Kθ , Kψ are proportional gains for z, θ̇+ = τθ · ∆t,
roll, pitch, and yaw. zd , ψd are the desired z position (5)
ψ̇+ = τψ · ∆t,
and yaw angle. z, ϕ, θ, ψ are the current z position
and orientation angles. R is the rotation matrix. ⃗a R = RotationMatrix(ϕ, θ, ψ),
    
is the acceleration vector. ⃗r¨, ⃗r˙, ⃗r are the acceleration, 0 0
velocity, and position vectors. ∆t is the time step. In ⃗a = R ·  0  −  0  /m,
comparison, the control vector comprises six elements T m·g
denoted by [zd , żd , ψd , ax , ay , az ]. These control inputs ¨
⃗r = ⃗a,
correspond to the desired z position, z velocity, yaw
angle, and x, y, z accelerations, respectively. As the ⃗r˙ + = ⃗r¨ · ∆t,
system’s complexity increases, the performance dispar- ⃗r+ = ⃗r˙ · ∆t
ity between our proposed planner and other planners
becomes more pronounced, as illustrated in Fig. 8. The results unequivocally indicate that our pro-

11
91
 Modeling, Identification and Control
posed planner achieves a considerably higher solve
rate compared to baseline planners. In this analy-
sis, we exclusively consider exact solutions, disregard-
ing any approximate results produced by the plan-
ners. Specifically, the solve rate achieved by our plan-
ner is 78%, markedly outperforming the next best-
performing planner, RRT*, which registers a solve rate
of 17%. The resulting path costs for all the plan-
ners that are able to provide an exact solution seem
reasonably similar. Based on these findings, we draw
that our planner is particularly advantageous for high-
dimensional systems requiring extensive sampling and
rapid state space exploration to obtain a solution. For
example, as illustrated in Fig. 8, existing planners ex-
cept RRT* and SST cannot navigate the sole pas-
sage (lower left) to the goal. As the number of sam-
ples increases, it becomes increasingly time-consuming
to identify the passage due to the growing number
of nodes in the tree structure. Our method’s bi-
directional approach offers benefits in navigating sharp
turns, preventing tree growth from becoming trapped,
and prolonging exploration. The proposed planner
achieves a threefold improvement in mean trajectory
cost compared to the next best-performing planner,
SST.
5.3 Discussion
The results distinctly demonstrate the advantages of
our newly introduced motion planner over current
state-of-the-art techniques, particularly in scenarios in-
volving a dynamic quadrotor characterized by high-
dimensional state and control spaces. The effectiveness
of our proposed planner can be attributed to several
pivotal characteristics:
Dynamic Balance between Exploration and Ex-
ploitation: The planner adeptly navigates between ex-
ploring new areas of the state space and exploiting
known paths. This is achieved through the strategic
selection of frontier vertices within the tree, which are
instrumental in guiding the exploration process, cou-
pled with the continuous refinement of existing solu-
tions.
Multi-threaded Bi-directional Tree Approach: A sig-
nificant strength of our planner is its implementation
of multi-threaded, bi-directional trees. This architec-
ture has proven particularly beneficial in enhancing
the solution rate for the dynamic quadrotor, outper-
forming traditional single-threaded or uni-directional
approaches in terms of both efficiency and effective-
ness.
These attributes underscore our planner’s robustness
and adaptability in complex motion planning scenar-
ios. The combination of an intelligent exploration-
exploitation balance and the innovative use of multi-
12
92
threaded bi-directional trees not only facilitates a
higher solution rate but also contributes to more nu-
anced and effective navigation through challenging
state spaces.
In comparing the benchmark results for the two sys-
tems, namely the Ackermann-steered robot and the dy-
namic quadrotor, a notable observation emerges. De-
spite allocating four times more time for the quadro-
tor, the solution rate for all planners, except ours, sig-
nificantly diminishes. This phenomenon underscores
the non-linear correlation between the state dimen-
sion complexity and the time required to find a solu-
tion. The exceptional performance of our planner can
be attributed to its inherent exploratory instinct and
the efficiency of its multi-threaded, bi-directional tree
search approach. These characteristics are pivotal in
effectively navigating the complex state space of the
quadrotor, thereby facilitating a higher solution rate
compared to other planners.
While our proposed motion planner exhibits notable
strengths, it is not without limitations, as reflected in
the solution rate results for both the Ackerman robot
and the quadrotor. In certain cases, the planner fails to
derive an exact solution. This limitation is suspected
to stem from the planner’s inherent tendency towards
extensive exploration. Before identifying an initial so-
lution, the planner prefers areas with fewer branches
and lower vertex density. Consequently, in scenarios
where numerous vertices are proximal to the goal yet
remain unconnected to the tree, the planner may in-
advertently prioritize frontiers distant from the goal
region. Over time, this reduces the likelihood of estab-
lishing a connection to the goal, as vertices near the
goal are less frequently selected as frontiers.
However, this limitation could be mitigated by im-
plementing goal biasing. Such an approach would in-
volve incorporating a proportion of the nearest neigh-
bors of the goal at each iteration of frontier selection.
This strategy could enhance the planner’s ability to
identify and prioritize goal-proximate vertices, thereby
improving the likelihood of establishing a connection
to the goal.
We have made the source code for our proposed mo-
tion planner, as well as the benchmarking code, pub-
licly available as open-source resources. This initia-
tive aims to facilitate further experimentation by re-
searchers and allows for the comparison of newly de-
veloped planners against our proposed method. From
a practical standpoint, users intending to deploy our
motion planner (or other baseline methods) must pos-
sess an understanding of the system model. Crucially,
they need to define a propagate function, which esti-
mates the next state of the system based on current
state parameters and control inputs. To assist users,
 Atas et.al., “CostTrust: A Kinodynamic Motion Planner”

we provide exemplar codes for two system models: an Karaman, S. and Frazzoli, E. Sampling-based al-
Ackermann-steered robot and a quadrotor. Addition- gorithms for optimal motion planning. CoRR,
ally, for effective utilization of the sampling-based mo- 2011. abs/1105.1186. URL http://arxiv.org/abs/
tion planner, a collision-check function is required to 1105.1186.
determine whether a given state is in a collision or is
collision-free. Karaman, S. and Frazzoli, E. Sampling-based optimal
motion planning for non-holonomic dynamical sys-
tems. In 2013 IEEE International Conference on
6 Conclusion Robotics and Automation. pages 5041–5047, 2013.
doi:10.1109/ICRA.2013.6631297.
This paper introduces a novel approach to kinody-
Kavraki, L., Svestka, P., Latombe, J.-C., and Over-
namic planning, showcasing enhanced performance
mars, M. Probabilistic roadmaps for path plan-
compared to existing state-of-the-art methods, partic-
ning in high-dimensional configuration spaces. IEEE
ularly in high-dimensional contexts such as dynamic
Transactions on Robotics and Automation, 1996.
quadrotors. Distinctive in its strategy, our method
12(4):566–580. doi:10.1109/70.508439.
employs a balanced interplay between exploration and
exploitation, complemented by a multi-threaded, bi- Kleinbort, M., Solovey, K., Bonalli, R., Bekris, K. E.,
directional tree search process. This dual-pronged ap- and Halperin, D. RRT2.0 for fast and optimal kin-
proach yields significant performance improvements, as odynamic sampling-based motion planning. CoRR,
evidenced by our results in two benchmarks. The po- 2019. abs/1909.05569. URL http://arxiv.org/
tential of our method to address real-world problems abs/1909.05569.
in autonomous robotics is evident from the results. In
certain instances, our planner may not yield an exact Kleinbort, M., Solovey, K., Littlefield, Z., Bekris,
solution, a limitation primarily attributed to its de- K., and Halperin, D. Probabilistic com-
votion to fast exploration of unknown spaces initially. pleteness of rrt for geometric and kinodynamic
To address this issue, future iterations of our planner planning with forward propagation. IEEE
will explore the incorporation of goal-biasing sampling. Robotics and Automation Letters, 2018. PP:1–1.
This enhancement would adjust the frontier selection doi:10.1109/LRA.2018.2888947.
criteria to balance the exploration with a more delib-
erate effort to connect to the goal in each iteration. Lavalle, S. and Kuffner, J. Rapidly-exploring random
Such a modification aims to refine the planner’s effec- trees: Progress and prospects. Algorithmic and com-
tiveness, ensuring a more targeted approach towards putational robotics: New directions, 2000.
achieving the desired goal state.
LaValle, S. M. and James J. Kuffner, J. Randomized
kinodynamic planning. The International Journal
of Robotics Research, 2001. 20(5):378–400. URL
References https://doi.org/10.1177/02783640122067453,
doi:10.1177/02783640122067453.
Gammell, J. D., Barfoot, T. D., and Srinivasa,
S. S. Batch informed trees (bit*): Informed Li, Y., Littlefield, Z., and Bekris, K. E. Asymp-
asymptotically optimal anytime search. The In- totically optimal sampling-based kinodynamic plan-
ternational Journal of Robotics Research, 2020. ning. CoRR, 2014. abs/1407.2896. URL http:
39(5):543–567. URL https://doi.org/10.1177/ //arxiv.org/abs/1407.2896.
0278364919890396, doi:10.1177/0278364919890396.
Littlefield, Z. and Bekris, K. E. Efficient and
Hauser, K. and Zhou, Y. Asymptotically op- asymptotically optimal kinodynamic motion plan-
timal planning by feasible kinodynamic plan- ning via dominance-informed regions. In 2018
ning in a state–cost space. IEEE Trans- IEEE/RSJ International Conference on Intelligent
actions on Robotics, 2016. 32(6):1431–1443. Robots and Systems (IROS). pages 1–9, 2018.
doi:10.1109/TRO.2016.2602363. doi:10.1109/IROS.2018.8593672.

Hsu, D., Latombe, J.-C., and Motwani, R. Path plan- Luna, R., Şucan, I. A., Moll, M., and Kavraki, L. E.
ning in expansive configuration spaces. In Proceed- Anytime solution optimization for sampling-based
ings of International Conference on Robotics and motion planning. In 2013 IEEE International Con-
Automation, volume 3. pages 2719–2726 vol.3, 1997. ference on Robotics and Automation. pages 5068–
doi:10.1109/ROBOT.1997.619371. 5074, 2013. doi:10.1109/ICRA.2013.6631301.

13
93
 Modeling, Identification and Control

Otte, M. and Correll, N. C-forest: Parallel short- Robotics and Automation (ICRA). pages 3191–3198,
est path planning with superlinear speedup. IEEE 2020a. doi:10.1109/ICRA40945.2020.9197338.
Transactions on Robotics, 2013. 29(3):798–806.
Strub, M. P. and Gammell, J. D. Advanced
doi:10.1109/TRO.2013.2240176.
bit* (abit*): Sampling-based planning with ad-
Perez, A., Platt, R., Konidaris, G., Kaelbling, L., and vanced graph-search techniques. 2020 IEEE
Lozano-Perez, T. Lqr-rrt*: Optimal sampling-based International Conference on Robotics and Au-
motion planning with automatically derived exten- tomation (ICRA), 2020b. URL http://dx.
sion heuristics. In 2012 IEEE International Confer- doi.org/10.1109/ICRA40945.2020.9196580,
ence on Robotics and Automation. pages 2537–2542, doi:10.1109/icra40945.2020.9196580.
2012. doi:10.1109/ICRA.2012.6225177. Sucan, I. A., Moll, M., and Kavraki, L. E.
The open motion planning library. IEEE
Shome, R. and Kavraki, L. E. Asymptotically opti- Robotics Automation Magazine, 2012. 19(4):72–82.
mal kinodynamic planning using bundles of edges. doi:10.1109/MRA.2012.2205651.
In 2021 IEEE International Conference on Robotics
and Automation (ICRA). pages 9988–9994, 2021. Xie, C., van den Berg, J., Patil, S., and
doi:10.1109/ICRA48506.2021.9560836. Abbeel, P. Toward asymptotically optimal mo-
tion planning for kinodynamic systems using a
Strub, M. P. and Gammell, J. D. Adaptively In- two-point boundary value problem solver. In
formed Trees (AIT*): Fast asymptotically optimal 2015 IEEE International Conference on Robotics
path planning through adaptive heuristics. In Pro- and Automation (ICRA). pages 4187–4194, 2015.
ceedings of the IEEE International Conference on doi:10.1109/ICRA.2015.7139776.

14
94
Paper V

From Simulation to Field: Learning

Terrain Traversability for
Real-World Deployment

Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

Submitted for Robotics and Autonomous System (RAS) Journal

95
Highlights
From Simulation to Field: Learning Terrain Traversability for Real-
World Deployment
Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

• A Novel Traversibility Estimation Neural Network: Our pro-

posed deep neural network is specially designed to process raw LIDAR
and IMU data, enabling continuous traversability assessments with spa-
tiotemporal point cloud maps. This approach showcases our network’s
ability to adapt to complex environments without needing real-world
data.

• Automated Data Generation and Labeling: We have pioneered

a high-fidelity simulation pipeline for autonomous data generation and
labeling based on the robot’s experiences across different terrains, sig-
nificantly reducing human intervention and facilitating large-scale data
processing for neural network training.

• Real-World Validation: Experiments on both simulated and real-

world platforms demonstrate the method’s superior predictive accuracy,
validating its efficacy in practical scenarios without prior real-world
data collection.

• Application and Open-Sourcing: The application of our traversabil-

ity assessments in 3D path planning algorithms confirms the method’s
reliability in generating collision-free paths. To further research and
practical application, we have open-sourced our neural network and
mapping code.

97
 From Simulation to Field: Learning Terrain
Traversability for Real-World Deployment
Fetullah Atasa , Grzegorz Cielniakb , Lars Grimstada
a
Faculty of Science and Technology, Ås, 1433, Norway
b
Lincoln Centre for Centre for Autonomous Systems, Isaac Newton
Building, Lincoln, LN6 7TS, Lincolnshire, UK

Abstract
The challenge of traversability estimation is a crucial aspect of autonomous
navigation in unstructured outdoor environments such as forests. It involves
determining whether certain areas are passable or risky for robots, taking
into account factors like terrain irregularities, slopes, and potential obsta-
cles. The majority of current methods for traversability estimation operate
on the assumption of an offline computation, overlooking the significant in-
fluence of the robot’s heading direction on accurate traversability estimates.
In this work, we introduce a deep neural network that uses detailed geometric
environmental data together with the robot’s recent movement characteris-
tics. This fusion enables the generation of robot direction awareness and
continuous traversability estimates, essential for enhancing robot autonomy
in challenging terrains like dense forests. To fulfill the data needs of the pro-
posed deep neural network, we present an innovative pipeline that utilizes
high-fidelity simulations for the automated generation and labeling of data,
dramatically reducing the need for exhaustive human involvement in data
collection and labeling.
The efficacy and significance of our approach are underscored by experi-
ments conducted on both simulated and real robotic platforms in various en-
vironments, yielding quantitatively superior performance results compared to
existing methods. Moreover, we demonstrate that our method, trained exclu-
sively in a high-fidelity simulated setting, can accurately predict traversabil-

Email addresses: fetulahatas1@gmail.com (Fetullah Atas),

gcielniak@lincoln.ac.uk (Grzegorz Cielniak), lars.grimstad@nmbu.no (Lars
Grimstad)

Preprint submitted to Robotics and Autonomous Systems December 10, 2023

98
ity in real-world applications without any real data collection. Our experi-
ments showcase the advantages of our method for optimizing path-planning
and exploration tasks within difficult outdoor environments, underscoring its
practicality for effective, real-world robotic navigation. In the spirit of col-
laborative advancement, we have made the code implementation available to
the public.
Keywords: Traversablity Estimation, Outdoor Robotics, Learning for
Navigation

1. Introduction
Field robotics often confronts challenges when operating within unstruc-
tured terrains as robustly determining the safe regions in such diverse fields
is a non-trivial problem. Such challenges arise in various applications, span-
ning mining, exploration, defense, agriculture, search and rescue, and beyond.
The essence of traversability analysis is to determine safe regions within the
environment robustly. Traversability assessment is a critical element for sev-
eral key operations of autonomous robots, such as formulation and refine-
ment of navigation plans. This study focuses on the robust traversability
assessment for robots in challenging outdoor settings, like forests or lightly
structured urban areas.
The task of evaluating traversability is inherently complex due to its de-
pendency on various factors, such as the robot’s design, the robot’s direction
(which continuously changes), and the characteristics of the terrain. Conse-
quently, a data-driven approach to traversability estimation requires careful
consideration. For example, accurate estimations for one set of robot spec-
ifications may become obsolete if there are alterations in the robot’s tire
dimensions or wheelbase. Acknowledging these dynamics, the methodology
for data-driven traversability must be adaptable rather than fixed to account
for potential changes in a robot’s capabilities.
Historically, researchers have used sensor modalities ranging from cameras
and LIDARs to proprioceptive sensing (e.g., IMU) to discriminate between
traversable and non-traversable regions. Each sensing technology presents
its own set of strengths and limitations. Our study uses a blend of LI-
DAR and IMU data to achieve an accurate, continuous value for deter-
mining traversability costs rather than a simple binary classification. This
blend, combined with an automated data generation and labeling strategy

99
 that exploits the robot’s locomotion experience over the terrains, enables
our proposed neural network to generalize well to output traversability es-
timates that are aware of the robot’s angle of approach, which we term as
directionality-aware.
Wallin et al. [1] recently proposed a traversability estimation technique
using simulated interactions between robots and terrain to produce train-
ing data and labels autonomously. Their method employs a neural network
trained on a Digital Elevation Map (DEM), enabling robots to navigate a
simulated environment, collect data, and refine their learning—a process ver-
ified in real-world conditions.
Building upon this foundation, our research advances the state-of-the-art
by introducing a data-efficient neural network that directly processes dense
point cloud maps and IMU data. The advantage of using point clouds lies
in their ability to accurately represent any real-world structure, unlike the
DEM that the previous method [1] depended on. This enhancement paves
the way for more precise and versatile traversability assessments in robotic
applications, which we demonstrate in the Sec. 4
Our approach enhances the concept of automated data generation and
labeling by utilizing raw sensory data from LIDAR and IMU, facilitating a
seamless transition from simulated environments to real-world terrain. This
direct sensor data utilization addresses the constraints of DEM-based meth-
ods, which can be limited in overlapping surfaces. Due to their single-height
limitation per location, DEMs often overlook potentially navigable spaces be-
neath tree canopies in scenarios like dense forests. Our method, by contrast,
presents a more comprehensive solution for autonomous navigation by ac-
counting for the full spectrum of environmental features that may otherwise
be concealed or inaccurately represented in DEMs.
The concern regarding LIDAR’s sparse output (compared to an RGB
image) is a significant one, as it traditionally hinders the ability to gener-
ate granular traversability assessments. Addressing this issue, our technique
synthesizes a densified, precise local point cloud map through the integration
of an Extended Kalman Filter (EKF) state estimate and a GPU-powered It-
erative Closest Point (ICP) algorithm [2, 3]. This enriched, robot-centric
map is critical in our methodology’s training and inference phases, ensuring
that our system can make fine-grained, accurate traversability estimations
beyond mere binary classifications.
Our research introduces the following core contributions in the realm of
traversability estimation for autonomous robot navigation through challeng-

100
ing environments:

1. We propose a novel deep neural network architecture tailored for traversabil-

ity estimation based on point cloud data alongside IMU information.
This architecture accommodates various point cloud attributes such as
normals and curvature, enhancing its adaptability to capture different
terrain characteristics.
2. We have established an automatic data generation and a ”proxy” data
labeling pipeline that leverages high-fidelity simulations and the robot’s
terrain locomotion experience, significantly reducing the necessity of
human labor in data collection and labeling.
3. We carry out thorough experiments utilizing both synthetic simulations
and real-world data, demonstrating the effectiveness of our approach
in real-world scenarios, particularly for path-planning tasks.
4. Unlike most existing methods, we supply the implementation of our
method. 1 . 2 .

2. Related Work
This section provides an overview of seminal contributions on traversabil-
ity estimation, categorized according to their methodological taxonomy.

2.1. Vision-Based Approaches

Initial work on traversability assessment primarily depended on vision-
based methodologies using traditional techniques. Stereo vision was founda-
tional for these methods to estimate the scene’s depth.
The authors in [4] introduced a classification algorithm leveraging stereo-
range data. This method considered both surface reflectivity and geometric
features derived from range measurements. The authors pointed out the
necessity for more comprehensive data to address variations in illumination.
Bajracharya et al. [5] employed a stereo system to generate dense voxel maps.
Their classification approach centered on geometric statistics of the RGBD
sensing. They utilized specialized illumination during the robot’s nighttime
operation to counteract the challenges of low-light conditions.

1
https://github.com/jediofgever/vox nav slam
2
https://github.com/jediofgever/traversablity estimation net

101
 Prior to the advent of deep learning, traditional machine learning paradigms
were at the forefront of research for traversability estimation. Notable con-
tributions include the work of Angelova et al. [6], who introduced a decision
tree classifier that was informed by features extracted from RGB images.
This model utilized Speeded-Up Robust Features (SURF) descriptors and
incorporated an analysis of various color variations to enhance its prediction
performance. In a related development, Bajracharya et al. [7] presented a
hybrid approach that combined the strengths of Support Vector Machines
(SVM) and naive Bayesian classifiers. This combination sought to lever-
age the robustness of SVMs and the probabilistic insights offered by naive
Bayesian methods, thereby enhancing the traversability classifier’s perfor-
mance in complex environments.
Traditional classifiers predominantly depended on hand-crafted features
and descriptors. This reliance posed challenges in terms of adaptability.
However, their minimal data requirement was a silver lining compared to
deep learning approaches. Kelly [8] compared a feature-driven SVM classifier
against a Convolutional Neural Network (CNN) for terrain classification.
Their results underscored the superior efficacy of the CNN-based approach.
Recent research has increasingly embraced the advantages of deep learn-
ing. For instance, Leung et al. [9] employed a deep CNN for the semantic
segmentation of images, enhancing the analysis of terrain traversability. They
also utilized data from Digital Elevation Maps (DEM) to identify traversable
regions. In a related endeavor, Hosseinpoor et al. [10] developed a deep se-
mantic segmentation model that processed aerial RGB images emphasizing
varying robot modalities. However, the authors did not conduct experiments
using either simulated or actual robots.
Bekthi et al. [11] presented a 2D imagery technique for terrain uniformity
evaluation utilized for end-stage traversability assessment. Their methodol-
ogy integrated motion indicators from inertial sensors with textures captured
from RGB images. The combined features derived from both sources were
input into a Gaussian process predictor. However, the authors highlighted
the method’s constraints in vast non-uniform environments, primarily due to
significant acceleration readings from the inertial sensor.
Chavez-Garcia et al. [12] explored terrain traversability through a heightmap-
based classification approach. They sourced their training data from terrains
that were procedurally generated using Perlin noise. Although the method
presented several strengths, it predominantly categorized terrains as simply
traversable or non-traversable. This constraint rendered the approach less

102
proficient in recognizing terrains with specific nuanced features, like ramps
that are steep yet passable.

2.2. LIDAR-Based Approaches

LIDARs, with their precise range measurements, have become a preva-
lent tool in terrain traversability assessment. In the subsequent sections, we
provide a concise overview of recent methodologies that primarily employ
LIDARs for estimating traversability.
Agishev et al. [13] introduced a point cloud geometry-based traversability
assessment technique to optimize navigation trajectories across unstructured
terrains. Their method uses statistical analysis of neighboring points to eval-
uate the slope and roughness of locations. Despite its strengths, the method
lacks directionality awareness, a critical aspect for dynamic environment in-
teraction. Nevertheless, this research is significant as it provides one of the
few publicly accessible traversability estimation results and datasets. It offers
a valuable baseline for comparative analysis, which we perform in Sec. 4.
Waibel et al. [14] proposed a traversability estimation methodology em-
ploying LIDAR and inertial sensing. They applied statistical methods to
gauge traversability measures, reminiscent of our previous work [15] and [13],
where geometric attributes of local terrain patches were used in assessing the
slope and roughness of the terrain.
Martinez et al. [16] showcased a traversablity classification technique for
LIDAR data using the Scikit-learn library. The authors highlighted the Ran-
dom Forest classifier’s efficacy in their approach. However, they also pointed
out computational inefficiencies in calculating point cloud features.
Thakker et al. [17] put forth a novel method for traversability analysis.
Their approach involves positioning the robot in specific poses and then
computing metrics derived from the surface point cloud and interactions
with the robot’s geometric structure. Notably, their method also predicts the
robot’s stability in these poses. One recognized limitation was the potential
misclassification of minor obstacles.
Xue et al. [18] unveiled an approach similar to ours in the sense that they
also use a robot-centric dense point clouds map. Their method integrates
consecutive LIDAR scans to construct a dense temporal map, which they
term ”LIDAR-based terrain modeling”. The traversability is inferred through
the geometric relationships between adjacent terrain sections, and they utilize
Normal Distribution Transform (NDT) for map generation. Contrasting this

103
 with our method, we incorporate inertial sensing and harness dense spatio-
temporal point cloud data processed by deep neural networks.

2.3. Hybrid Approaches

Several approaches integrate RGB images and LIDARs, occasionally fur-
ther augmented with inertial or tactile sensing. Below, we spotlight some
notable methods using multi-modalities.
Wallin et al. [1] introduced a method that can be considered a precursor
to our current work. The authors utilized high-resolution topography data
for traversability prediction, an approach akin to ours where traversability
is modeled as a regression problem. Additionally, they employed simulators
for data collection to train their deep neural networks. However, a limitation
of this approach is its reliance on a pre-existing map of the environment in
real-world scenarios, potentially constraining its practical applicability.
Fusaro et al. [19] adopted an SVM classifier to gauge the traversability
of terrains, integrating both LIDAR and RGB image data to achieve an
accuracy of 89.2%. While this method marks a significant step in terrain
analysis, it’s important to note that their evaluation was primarily focused
on urban contexts, characterized by relatively uniform terrain and infrequent
significant elevation changes. This urban-centric approach leaves room for
further exploration in more varied and challenging environments.
Building upon the need for more comprehensive terrain analysis, Zhang
et al. [20] introduced an approach that extends beyond urban landscapes.
Their methodology leverages several point cloud patch features, including
roll, pitch, and roughness, combining geometric features while also incorpo-
rating the robot’s suspension model. This method is particularly notable
for its emphasis on enhancing pre-existing motion plans. However, it pri-
marily improves upon existing navigational strategies rather than offering a
deepened understanding of terrain traversability per se.
To address the limitations of previous methods in dealing with complex
terrains, Haddeler et al. [21] recently proposed a terrain-probing method that
is especially effective in challenging environments like soft soil, puddles, and
bushy areas. Their innovative approach bridges geometric and appearance-
based sensing, offering a more holistic view of terrain analysis. Despite its
advancements, the method has limitations, particularly its reliance on tactile
sensing mechanisms, which may restrict its applicability in broader outdoor
scenarios.

104
 Guan et al. [22] introduced a method for traversability mapping utilizing
LIDAR point clouds combined with RGB images. This technique integrated
3D volumetric traversability metrics to develop a global map, primarily to
aid in the planning and navigation of an excavator. However, the authors
reported limited system testing due to safety concerns. It remains unclear
whether the method is apt for long-range navigation, as the paper does not
provide explicit details in this regard.
Readers are encouraged to consult the comprehensive survey paper by
Borges et al. [23]. This work provides an in-depth examination of various
traversability analysis methods, categorizing them based on their sensing
modalities and taxonomies.
Recent advancements in traversability estimation research increasingly
favor deep learning over traditional learning methods. While often requiring
less data, traditional techniques face challenges in terrains lacking distinct
features, as Kelly [8] points out. In response, our study introduces a deep
neural network architecture designed to determine continuous traversabil-
ity values across varied terrains. We utilize an automated data generation
method to address deep learning’s data inefficiency based on high-fidelity
simulation environments. Additionally, our approach uniquely integrates in-
ertial sensing with spatiotemporal LIDAR point clouds, generating a detailed
point cloud map to infer traversability estimates around the robot. These
traversability estimation maps enhance local motion planning and control,
ensuring safer navigation.

3. Approach
In this section, we provide an in-depth explanation of our methodology.
Our approach harnesses data from LIDAR and IMU sensors to generate
detailed, robot-centric traversability point cloud maps.
A framework of the proposed method is depicted in Fig. 1. Our sys-
tem encompasses several key elements: the TraverseNet, a sophisticated
LIDAR-Inertial neural network architecture, the auto-generation of a dataset
using high-fidelity simulations, and the construction of robot-centric point
cloud maps. The following sections will elaborate on each component of our
methodology, coupled with a precise problem statement to ensure clarity.

105
 Figure 1: Comprehensive Traversability Estimation Pipeline: Utilizing sensor inputs and
high-fidelity simulations, data is processed via the EKF State Estimator and fed into Tra-
verseNet. This deep neural network, trained on autonomously generated datasets, outputs
precise traversability estimates vital for outdoor autonomous navigation components such
as planning, control, and costmap.

3.1. Problem Statement

Our work addresses the challenge of determining traversability for au-
tonomous robots in unstructured environments. We define traversability T
as a function that assigns continuous values to the terrain, given the robot’s
configuration C, and local terrain patch state S. Precisely, T maps S and C
to a continuous value in the range of [0, 1]: T (S, C) →
− [0, 1]
In this context, S is the local terrain patch state represented by point
clouds, which includes spatial properties that affect navigation, and C is the
robot’s configuration, incorporating attributes such as its orientation and
recent locomotion characteristics.
The traversability (cost) score ranges from 0, signifying fully passable, to
1, indicating non-traversable. The task is to estimate T accurately for any
pair of S and C, necessary for effective path planning and navigation across
diverse applications. To tackle this, our approach centers on a deep neural
network and an automated data generation system that simulates various
terrain states S and robot configurations C. This dataset trains our newly

106
proposed neural network fθ , which aims to approximate the traversability
function T : fθ (S, C) ≈ T (S, C) The network’s parameters, θ, are optimized
to minimize the discrepancy between the network’s output and the actual
traversability scores derived from simulations. This optimization enhances
the robot’s ability to interpret its environment and make informed navigation
decisions respecting the terrain traversability.

3.2. LIDAR-Inertial Traversability Estimation: TraverseNet

PointNet, introduced by Qi et al. [24] in 2017, revolutionized the han-
dling of raw point clouds by deep learning networks, later leading to a wave
of improvements such as PointRCNN [25] and PointNet++ [26]. Currently,
no PointNet derivatives have been tailored for traversability estimation, an
application with promising advantages; Kelly at al. [8] have already reported
the superior performance of deep neural nets over the traditional approaches,
given that they are provided enough data. Taking a cue from PointNet, our
model illustrated in Fig. 2 brings a critical enhancement: it is intrinsically
sensitive to the robot’s directional orientation, an aspect frequently over-
looked by conventional models.
For the proposed neural network, we explored various architectural de-
signs, layer sizes, and input feature integration, including direct and inter-
mediary fusion of IMU data. The results, detailed in the accompanying
comparison table (Tab. 1), reveal the efficacy of our approach. Our neural
network’s standard inputs are point features (e.g., coordinates, normal vec-
tors, curvature) alongside a 13-dimensional IMU covariance and orientation
vector. We experimented with two main strategies for integrating IMU data:
’direct fusion,’ where IMU features are concatenated with latent point cloud
features without additional processing (see Fig. 2), and ’mid-fusion,’ where
IMU features are first processed through convolutional and pooling layers
before being combined with the already processed point features (see Fig. 3).
Although this paper only details two of such architectures for brevity (see
Fig. 2 and Fig. 3), comprehensive performance data for various configurations
can be found in the experimental results section, see Tab. 1.
Traversability-aware navigation is a complex task that goes beyond mere
obstacle detection; it requires robots to assess how their directional move-
ments might affect their ability to traverse different terrains. Traditional
geometry-based methods for terrain analysis [15, 18], despite their sophisti-
cation, typically do not account for the differences in effort required to move

10

107
 uphill versus downhill, often simplifying traversability into a static measure.
This approach neglects the reality that climbing usually requires more power.
Our model advances beyond this limitation by incorporating the robot’s
angle of approach into traversability calculations, enabling a dynamic eval-
uation that aligns with the practical challenges encountered across diverse
landscapes. This enhancement is achieved through the data labeling tech-
nique rooted in the robot’s extensive locomotion experience, reflecting the
varied demands of different terrain types. The subsequent subsection delves
into the details of our locomotion-based data labeling method, shedding light
on the way a robot’s interaction with its surroundings shapes the nuanced
traversability estimates our model provides.

Figure 2: Fundamental TraverseNet Design: This illustration presents the core architec-
ture, utilizing point coordinates data for feature extraction through convolutional and fully
connected layers, subsequently integrated with a 13-dimensional IMU feature vector.

11

108
Figure 3: Optimal TraverseNet Configuration: Detailed in Section Sec. 4, the architecture
utilizing point coordinates (x, y, z) and point curvature inputs exhibits notable perfor-
mance. Unlike design with direct IMU fusion depicted in Fig. 2, this approach processes
IMU data through additional feature extraction before concatenating it with LIDAR fea-
tures in the final layer.

3.3. Locomotion as Traversability Indicator

Figure 4: An illustration depicting the robot’s computation of traversability labels using

both nominal and actual travel distances. The blue represents the robot’s actual path,
the gold symbolizes the anticipated nominal path, and the green displays a time-series
representation of the computed IMU acceleration covariance.

Deep learning methods, while effective, necessitate the provision of data

and corresponding labels in the context of supervised learning. In our method-

12

109
 ology, we treat terrain traversability as a regression problem. Therefore, for
each data sample, the neural network requires both the sample and its asso-
ciated traversability cost label. We utilize the robot’s experience across the
terrain to fulfill this demand.
With specific physical attributes such as wheel radius and wheelbase,
and a defined fixed speed, one can anticipate the nominal distance a robot is
expected to traverse within a set time frame. Yet, challenging terrains, like
steep slopes or obstacles, can cause deviations in the actual traveled distance
from this estimation. Using the difference between the expected and actual
distances within a windowed time period, we can obtain a robust, continuous
traversability cost label. For example, while descending a slope, gravity might
enable the robot to cover more distance than predicted. Conversely, if the
robot encounters an obstacle like a rock and gets stuck, the traversability
cost at that region might peak at 1.0, its maximum value. The illustration
in Fig. 4 provides a visual representation of this concept, highlighting the
disparity between predicted and actual distances.
Additionally, our approach factors in past IMU readings, specifically ex-
tracting the acceleration covariance, which serves as neural network input.
The nominal distance dn is computed as:

dn = ((r.w1 ) + (r.w2 )) · t (1)

Here, r denotes the wheel radius, w1 and w2 represent the left and right
wheel speeds and t is the designated period of time. By default, t is 3 sec-
onds unless otherwise specified. Calculating the actual traveled distance, da ,
is straightforward, drawing from the robot’s state estimation that employs
an Extended Kalman Filter (EKF) to integrate IMU, wheel odometer, and
GNSS data. The primary purpose of the EKF state estimation is to furnish
an initial estimate for ICP-based point cloud mapping. While not strictly
essential, the availability of GNSS data could benefit long-term operations,
especially during the testing phase. This is because robot movement is rel-
atively restricted in the training phase (three minutes max), making GNSS
support advantageous for extended operational periods.
We maintain label consistency by ensuring that the difference between
nominal and actual distances (denoted as dn − da ) falls within the 0 to 1
range. This is accomplished by clipping values that fall outside this range to
their respective minimum or maximum limits, depending on whether they are
below the minimum or above the maximum threshold. As our strategy also

13

110
integrates IMU data, we include the acceleration covariance from the preced-
ing t time and roll, pitch, and yaw angles in the inertial frame. To simplify
input for the neural network, orientation is represented using quaternions.
Our IMU feature vector is 13-dimensional; 9 elements originate from the flat-
tened acceleration covariance matrix Q, while the remaining four belong to
the quaternion vector H.

3.4. Environments and Data Generation Process

Figure 5: A sequence of snapshots taken from a high-fidelity forest environment during

a robot’s data collection journey. Starting at the upper left, the robot progresses to its
final position in the bottom right, where it encounters a non-traversable rock, marking
the conclusion of a data collection ”episode”. The locomotion-based traversability, as
discussed earlier, is implemented. Data samples are taken at intervals, ensuring the robot
covers at least 1 meter between each data snapshot, aligning with the traversability label.

To cater to the data requirements of our neural network, we employ high-

fidelity simulations using the Unity Game Engine, renowned for its perfor-
mance and geometric and visual precision. Our data collection hinges on two
primary environments:
1. A forest environment, not based on any real-world location, spanning a
1km2 area. It boasts intricate vegetation, diverse terrain, and various objects
such as trees, rocks, bridges, rivers, and gravel roads.
2. A real-world replica covering parts of the Norwegian University of
Life Sciences campus and adjacent residential areas in As City, Norway. We

14

111
 construct this environment in the Unity Editor using C# scripts, leveraging
elevation data, road structures, and other map objects from the Open Street
Map (OSM) dataset. It, too covers a 1km2 area.
These detailed environments ensure we gather comprehensive data for
both urban and outdoor settings.
The actual data and label collection proceeds in two distinct phases:
First, we deploy multiple robots for efficiency. Each robot is equipped
with LIDAR, IMU, Camera, and GNSS sensors, with their data relayed to
Robot Operating System (ROS) [27] topics.
Leveraging the gathered sensory data, we implement an Extended Kalman
Filter (EKF) as a state estimator, enabling us to generate a robot-centric lo-
cal point cloud map accurately. This process is elaborated in the subsequent
subsection. Using the robot’s tracked locomotion quality as a basis for the
traversability indicator, we determine the traversability cost label crop points
directly below the robot and ascertain the IMU covariance matrix. In the
procedure, every sample is stored on the disk with a timestamp, embedding
the traversability cost label with three-decimal precision in addition to saving
the disk text file including a 13-dimensional IMU feature vector.
Operating multiple robots simultaneously is critical in our data collection
methodology. Our experimental setup employs a gaming computer with 32
GB RAM and an NVIDIA 2060 4GB GPU. This configuration permits the
concurrent operation of up to 10 robots without any detrimental impact on
the frame rate. It is important to recognize that this figure is inherently tied
to the computational capabilities of our specific setup, particularly regarding
the demands of simulating a 64-ring LIDAR sensor for each robot.
The automated data generation steps are vividly depicted in Fig. 5, where
the anticipatory behaviors of the robots within the simulation are illustrated.
Furthermore, Fig. 6 expands on this by showcasing the multi-robot data col-
lection process in a life-like simulated environment. These visual represen-
tations provide a clear understanding of the sophisticated data generation
pipeline that underpins our research.

3.5. Generation of Dense Robot-Centric Point Cloud Maps

LIDAR is commonly used to gather precise measurements of the sur-
roundings, though it often provides sparse data compared to the dense in-
formation from RGB camera images. A single LIDAR scan might be enough
for tasks such as identifying objects or binary categorization of areas as
traversable or not. However, when assessing the detailed ”traversability”

15

112
Figure 6: The image depicts three robots (highlighted within yellow circles) navigating
through a simulated high-fidelity forest environment and collecting data alone. The LIDAR
point clouds of each robot are also depicted in distinct colors. It is important to note that
these robots operate concurrently and can physically interact, with potential collisions and
the possibility of one robot’s body being detected by the sensors of another. The robots
are spawned at random locations with random orientations but move with a fixed linear
speed of 1 meter per second throughout each episode.

cost of an area with a continuous value, a single scan may fall short. That’s
why we use point clouds that accumulate data over a windowed time, moving
along with the robot, making them robot-centric.
We employ the Iterative Closest Point (ICP) algorithm for generating
dense point clouds, benefiting from its efficient GPU performance [2]. This
method not only improves the accuracy of the robot’s state estimation but
also effectively tracks movement on steep slopes, creating detailed local point
cloud maps. Our approach is also compatible with established SLAM tech-
niques like LIO-SAM (Shan et al. [28]). However, while these techniques are
reliable in many scenarios, they face challenges on highly uneven terrains.
For instance, the reliance on IMU pre-integration can lead to misinterpre-
tation of gravitational forces as movement when a robot is stationary on a
slope, which is why we did not prioritize this approach.
Loop closures, which help correct the robot’s trajectory and avoid drift in
traditional SLAM systems, are not crucial for our purpose. This is because
our robots generally move in a consistent direction until an obstacle stops
them or a set time limit is reached (in the data collection phase). For data
collection, we typically let each robot episode run up to a maximum of three
minutes to ensure a balanced dataset.

16

113
 4. Experiments
4.1. Platform, Sensors, and Setup
The real robot platform we use for real-world experiments is a mid-sized
AGV from Robotnik with active-front-and-rear-steering, see Fig. 7. To facil-
itate data collection, we constructed a simulated skid-steered replica of this
robot within the Unity game engine. Details of our simulated robot are as
follows:
The replica model ”Dobbie” is designed to navigate uneven terrains. It is
equipped with a 64-ring Ouster LIDAR and an XSENS IMU. The platforms
feature additional sensors, including cameras and a GNSS system, though
these were not leveraged in developing our traversability estimation frame-
work. While the steering kinematics are not identical, they possess same
sensor suite with comparable geometric dimensions and ground clearance.

Figure 7: Robotic platforms used for experimentation. The red-bordered images depict the
simulated robot model, whereas the green-bordered images provide multiple perspectives
of the actual Robotnik AGV platform. For our method and experiments, the primary
sensors employed are 3D LIDAR and IMU; other sensors on the platform were not directly
utilized for traversability estimation.

17

114
4.2. The Dataset and Data Augmentation
Our dataset was generated through an automated approach; we created
a series of C# scripts to achieve this. We first position robots at random
coordinates within a 1km x 1km environment in Unity. Each robot was
oriented randomly and maintained a fixed linear speed of 1 meter per second.
The movement continued for either a maximum duration of three minutes
or until the robot became immobilized due to obstacles or tipping incidents.
We achieved these programmatically by observing the ground truth pose of
robots accessible within the simulation.
Throughout the robot’s trajectory, our scripts captured snapshots. These
comprised a cropped point cloud segment, highlighting the robot’s expanded
footprint (1.0, 0.67, 1.0) along the x, y, and z dimensions. Accompanying this
was the IMU acceleration covariance based on the previous three seconds and
the most recent IMU orientation within the inertial frame. Thus, each sample
incorporated a point cloud segment of the robot’s footprint, the IMU’s 3 x 3
covariance matrix, and a quaternion representation of the inertial orientation.
The IMU data was simulated based on the rigid body acceleration of the
robot.
To achieve a comprehensive representation of various terrain situations,
especially those involving non-traversable regions that are inherently difficult
to access, we employed manual teleoperation. This approach facilitated the
navigation of robots into such challenging areas. Subsequently, a ROS service
was utilized to initiate the capture of samples in these specific locations,
ensuring a balanced dataset.
To augment the diversity of our dataset, we employed two techniques
specifically for augmenting point cloud samples while leaving the IMU data
untouched. First, we downsampled the point cloud using a random voxel size
selected from the range of 0.05 to 0.25. Second, we introduced zero-mean
Gaussian noise with a standard deviation of 0.05 to the point cloud sam-
ples. We randomly selected augmentation samples, constituting half of the
entire training set. Consequently, post-augmentation, the dataset expanded
by a factor of 1.5X. All point cloud samples were subsequently normalized
to fit within a metric sphere, while the IMU data remained without any nor-
malization as the 13-dimensional IMU features vector ended up having an
intrinsically structured range.

18

115
 Features Final Loss Train MAE Test MAE
XYZ 4.1 0.05 0.030
XYZ + N. 4.0 0.05 0.035
XYZ + C. 4.3 0.07 0.033
D-F IMU + XYZ + N. 4.2 0.08 0.032
M-F IMU + XYZ 3.7 0.05 0.032
M-F IMU + XYZ + C. 3.8 0.07 0.024
M-F IMU + XYZ + N. 3.5 0.06 0.034

Table 1: Training results on automatically generated traversability dataset, the results for
different feature combinations and IMU data fusion strategies are presented. The train
MAE numbers are peak values during the training. The Test MAE values were retrieved
after 300 epochs of training.

4.3. Metrics and Evaluation Considerations

We employ the L1 loss function for our neural network training due to its
robustness to outliers. Furthermore, to assess the accuracy of the predicted
traversability costs, we utilize the Mean Absolute Error (MAE) metric given
by:
n
1X
MAE = |yi − ŷi |, (2)
n i=1
where ŷi predicted value, yi is the actual label and n is number of samples. We
conducted an ablation study to assess the distinct impacts of input features
and the incorporation of IMU sensing on our model’s overall performance.
For example, we included the point normals as additional features to the
neural network to see the impact. The performance metrics are presented
using MAE values presented in Tab. 1.

4.4. Feature Impact Analysis on Model Performance

In this subsection, we showcase the impact of various combinations of
point cloud features and IMU data on the model’s performance. Additionally,
we report the MAE values derived when evaluating the model on previously
unseen samples from the test set.
In 1, we detail the outcomes derived from different feature combinations.
Here, ”N.” signifies normals, and ”C.” represents the curvature of the point.
When utilizing Direct Fusion (D-F), we concatenate the IMU sensing directly
with the processed point cloud features as depicted in Fig. 2. In contrast, for

19

116
Mid-Fusion (M-F) (see, Fig. 3), we first extract features from the IMU data
using fully connected layers, then fuse these processed features for subsequent
prediction tasks.
Discussion: Analyzing the presented results, it is evident that the model
”M-F IMU + XYZ + C” stands out with the final loss of 3.8 and the lowest
end MAE of 0.024, making it the most accurate by the conclusion of the
training. Interestingly, its performance surpasses even its more feature-rich
counterparts, such as ”M-F IMU + XYZ + N.” This suggests that integrating
additional features did not always equate to enhanced performance. For
instance, incorporating normals (3-dimensional feature) to the base ”XYZ”
model results in only marginal improvements or, in some cases, even slight
regressions in metrics. The optimal balance between model complexity and
performance appears to be achieved with the ”M-F IMU + XYZ + C” model,
making it a promising choice for the rest of the experiments. Furthermore,
the application of Mid-Fusion—entailing additional feature extraction from
IMU data prior to concatenation with LIDAR features—appears to enhance
the model’s performance.
We experimented with L1 and L2 loss functions during the neural network
training phase. We observed that L1 loss facilitated better convergence and
exhibited greater robustness to outliers within the dataset. For optimization,
we utilized the ADAM [29] algorithm, with a learning rate of 0.001.

4.5. Qualitative Results of Traversability Estimation

In this subsection, we detail and interpret the results of our traversabil-
ity estimation in highly detailed simulation environments. As depicted in
Fig. 8, the left side visualizes the robot and its environment, while the right
side colorfully represents the resulting traversability estimates. When com-
paring the estimates in Fig. 8(a) to those in Fig. 8(b), the former appears less
smooth. This distinction arises from employing two different inference meth-
ods, as further explained in Fig. 9. Specifically, Fig. 8(b) exhibits a smoother
traversability estimate, which aptly allocates lower traversability costs to the
more planar regions of the environment. In contrast, while Fig. 8(a) shows
the robot navigating a narrow bridge and provides associated traversability
estimates, its accuracy is not on par with the results in Fig. 8(b).
The size of the robot’s footprint dimensions plays a pivotal role in our
methodology, as it essentially determines a resolution for the traversability es-
timation map. In Fig. 10, we illustrate the influence of varying footprint sizes
on the outcomes. Our proposed neural network architecture was specifically

20

117
 trained with footprint dimensions of (1.0, 0.67, 1.0) for the x, y, and z axes,
respectively. Even though optimal results are anticipated when using this
specific footprint size, our method remains adept at predicting traversability
estimates with changing footprint sizes (given that the ratio of dimensions
stays similar). This robustness is attributed to normalizing points within the
robot’s footprint to a unit sphere during the training and inference stages.
In Fig. 10(a), we demonstrate the ability to retrieve traversability estimates
even with a reduced footprint size of (0.25, 0.16, 0.25). This capability is at-
tributed to our method of aggregating the spatiotemporal LIDAR scan into
a dense, robot-centric map that rolls with the robot.

21

118
 (a)

(b)

(c)
Figure 8: The figure illustrates traversability estimations in two distinct environments: a
bridge and a random forest patch. In part (a), the robot’s local vicinity is discretized to fit
within its footprint. In part (b), while the environment is again discretized based on the
robot’s footprint, the inference is conducted on a finer grid resolution. This approach fuses
multiple traversability estimations, yielding results that are more accurate and smoother
in their presentation. In (c), blue indicates areas that are easily traversable, green suggests
moderate challenges, and red denotes difficult or impassable regions, the range within (0,
1).

In Fig. 12, we detail the outcomes from the neural network using a fine-
grained step size configuration, as depicted on the right side of Fig. 9. The
results for varying robot footprint sizes reveal that smaller footprints make
the method less assertive in pinpointing high-cost areas, possibly due to the
limited point information and variation in these smaller regions. As the

22

119
 footprint size expands, the method exhibits heightened confidence in distin-
guishing clearly non-traversable zones, particularly evident in Fig. 12(d).

Figure 9: Two distinct strategies were employed for inferring traversability estimates using
the neural network. In the left setup, the environment is discretized based on the robot’s
footprint size. Conversely, in the right setup, the environment is segmented using the
robot’s footprint but with a notably smaller step size, resulting in overlapping regions.
These overlaps allow for multiple predictions at the same location, enhancing the accuracy
and smoothness of the estimates, albeit at the cost of increased computation time.

Fig. 11 showcases additional experimental outcomes from deploying the

trained neural network in high-fidelity simulations, illustrating the efficacy
of LIDAR in low-light and shadow-prone forested terrains. In the top-right
section, the network’s capability to discern the ’rough’ segments of a gravel
path is evident, where higher yet navigable regions are marked in distinct
colors upon the rocks, encapsulated by a yellow contour for emphasis and
comparative analysis.

23

120
 (a) (b)

(c) (d)

Figure 10: Figures display traversability estimates for robot footprints with a consistent
x:y:z ratio as the original footprint of (1.0, 0.67, 1.0). The x dimensions are 0.25 in (a),
0.5 in (b), 0.75 in (c), and 1.0 in (d).

Figure 11: TraverseNet Evaluation in High-Fidelity Simulation: The top-left image

presents the robot’s view, while the top-right shows traversability estimations overlayed
onto this view, allowing for method accuracy verification. The bottom images display the
3D traversability estimation point clouds, with discretized environmental boxes highlight-
ing spatial delineations.
24

121
 (a) (b)

(c) (d)

Figure 12: Figures show the effect of robot footprint dimensions on traversability, with x
values of 0.25, 0.5, 0.75, and 1.0 in (a) through (d) and a consistent x:y:z ratio. Differing
from Fig. 10, an overlapping box approach is used, refining the estimate precision.

Discussion: Our method’s proficiency in handling diverse scenes di-

rectly, without the need for additional tuning or real-world data, stands out
as a key finding in our qualitative assessment. The method employs densified
point cloud maps that facilitate detailed traversability estimates, proving es-
pecially beneficial for challenging terrain textures and navigating low-light
conditions. Moreover, the introduction of overlapping boxes in our approach
has proven to refine coarse estimates, resulting in smoother estimates that
are favored in path planning and robotic control systems.
Despite the above strengths, our method is not without limitations. It
involves certain parameters, such as the size of the time window for IMU
covariance computation and the set linear velocity, which are not trivial to
optimize. Alterations to these parameters necessitate a re-collection of data,
posing a challenge to the method’s adaptability.

4.6. Comparision to state-of-the-art

We have drawn a comparison between our method and that of Wallin
et al. [1]. Both techniques gauge traversability by assessing the disparity
between the actual traveled distance and the anticipated distance predicated

25

122
 Method Data Input Formulation Metric Performance Directionality
Our P. Clouds Regression MAE 0.024-0.035 Y
Wallin et al. [1] DEM Regression MAE ≈0.2 Y
Xue et al. [18] P. Clouds Classification Precision 97-99% N
Agishev et al. [13] P. Clouds Classification - - N

Table 2: Comparative Analysis of Various Traversability Estimation Methods and Their

Corresponding Performance Metrics: This table summarizes the performance values of
each method as reported in their respective studies, evaluated on the datasets used in
those papers.

on robot motion parameters. Our method directly processes point clouds, in

contrast to Wallin et al., who employ a discrete elevation map. Their reported
Mean Absolute Error (MAE) stands at 0.2 for a 1.0-meter grid resolution. In
contrast, our findings, detailed in Tab. 1, indicate an MAE of 0.024, marking
a considerable enhancement in accuracy for the same resolution.
Furthermore, we evaluated the outcomes achieved by Xue et al. [18].
This method, which doesn’t rely on learning, operates on spatiotemporally
aligned point clouds similar to ours. A crucial distinction is that Xue et
al. [18] evaluate traversability estimation as a binary classification task. They
quantify their metric, termed ”precision”, as the ratio of true positives to the
sum of true and false positives. The precision they report falls within the
97-99% range.
To evaluate the efficacy of our approach, we further evaluated our method
on an external dataset of real-world forest environment data provided by
Agishev et al. [13], which is discussed in the subsequent section.
The ambiguous nature of traversability estimation, influenced by factors
such as sensor types, varying problem formulations (classification or regres-
sion), and the capabilities of different robot platforms, presents a challenge
in establishing a standardized benchmark for comparison with the leading
methods. However, the dataset released by the authors of [13] serves as a
suitable candidate for benchmark evaluations. The robot platform they used
in the experiments poses locomotion capabilities similar to our robot model.
In adapting our approach for this comparison, we adjust our method to yield
values aligning with their model’s binary output. It is important to note that
the dataset includes labels generated by Agishev et al.’s[13] method rather
than manually verified ground truth. Therefore, the comparative results
shown in Fig. 13 are intended for qualitative analysis rather than quanti-
tative validation, as there are no ground truth labels for quantifying both
method’s accuracy.

26

123
 Figure 13: The figures showcase the efficacy of our traversability estimation method when
applied to an external forest dataset provided by Agishev et al. [13]. Our approach yields
enhanced accuracy even without IMU data integration. This improvement is especially
evident in resolving coarse binary classifications of objects like tree trunks and pedestrians,
where our method offers more accurate traversability assessments. Visual comparisons are
provided by highlighting our method’s estimates in blue boxes, contrasting with the orange
boxes used for Agishev et al.’s estimates.

Discussion: Utilizing point clouds offers a distinct advantage due to

their ability to capture dense and raw measurements, especially when they
are aligned over time using techniques like ICP or NDT, as seen in both our
method and that of Xue et al. [18]. Unlike elevation maps, which provide a
simplified view, point clouds capture the full geometric intricacies. This de-
tailed representation might be a significant factor in the enhanced accuracy
demonstrated in Tab. 2. Although the results from Xue et al. [18] are note-

27

124
worthy, it is important to highlight that their method views traversability in
simpler terms: it is either traversable or not. Moreover, they do not take into
account the robot’s direction. This distinction is vital; for example, the chal-
lenge of moving down a slope with a 10% grade is different from moving up
the same slope. The reported performance values are derived directly from
the authors’ conclusions as stated in their respective sections. Each method
underwent evaluation on its own distinct dataset; the absence of accompa-
nying implementations and different sensor modalities constituted a barrier
to the replication of the reported performance outcomes.
Upon analyzing the results from the external dataset by [13], our method
demonstrates a more cautious and detailed performance. For example, re-
gions at the base of tree trunks and portions of the pedestrian point clouds,
which were often marked as traversable by [13], were correctly identified as
non-traversable by our method. It is noteworthy, however, that our method’s
conservative bias may lead to mistakenly categorizing steep slopes as non-
traversable, as depicted by the images in Fig. 13.

4.7. Real-world Experiments

In our study, we performed real-world experiments using the Robotnik
platform depicted in Fig. 7. Despite training the neural network exclusively
on simulated data, we observed impressive generalization during field tests;
the model adeptly estimated traversability without requiring additional fine-
tuning. We conducted these tests on the campus of the Norwegian University
of Life Sciences, an outdoor setting featuring varied elevations, and sections
of asphalt and gravel roads. The derived traversability estimates, integrated
into the environmental map, are overlayed onto a satellite image in Fig. 14.
Our real experiments indicate that our method can produce a locally
consistent, dense traversability map within acceptable time frames. We trim
the SLAM-generated map to focus on the robot’s immediate vicinity using a
3D box defined by corners (-10, -10, -5) and (10, 10, 5). Using the overlapping
box configuration shown on the right of Fig. 9, we generate around 200
boxes, discarding those boxes encapsulating fewer than three points. The
neural network processes this in approximately 0.2 seconds. Using the non-
overlapping box configuration depicted on the left of Fig. 9 can enhance
processing speed to 50 FPS, though at the expense of estimation accuracy.

28

125
 Figure 14: The traversability estimates are merged with the point cloud map generated
using LIO-SAM [28], a 3D SLAM approach. The method accurately differentiates between
solid obstacles like trees and buildings. Additionally, it designates higher traversability
costs to areas with inclines or gravel, as indicated by the green patches). The integrated
traversability map is overlayed onto Google Earth’s satellite imagery for reference.

Figure 16: Snapshots from various moments during an outdoor experiment. As in Fig. 15,
the image-point pairs are provided where RGB colors of points indicate traversability
costs. The figure also demonstrates the method’s directionality awareness, assigning higher
traversability costs and a larger area of concern when approaching an uphill sidewalk bump
(b), as opposed to a less conservative estimate when descending from the same sidewalk
(c). The relevant regions have been marked with yellow contours in images (b) and (c) for
clarity.

29

126
Figure 15: A sequence showcasing the method applied to a real scene, featuring both
image-point cloud views. The top images overlay the traversability point cloud map on
actual images for visual validation. The bottom images present 3D visualizations of the
traversability estimation within the point cloud, where traversability costs are embedded
in RGB values.

Discussion: Our method effectively identifies areas with subtle sidewalk

bumps, fences, and tall vegetation, as illustrated in Fig. 16. Moreover, the
distinction between gravel and asphalt roads in terms of traversability esti-
mates is apparent in Fig. 14, likely influenced by the inclusion of IMU data.
However, the method encounters challenges with dynamic obstacles. For in-
stance, in Fig. 16, a pedestrian, once captured in the map but later moved,
remains depicted in the traversability estimate.

30

127
 4.8. Path Planning and Navigation Experiments

Figure 17: Snapshots capture two instances where the point cloud-based path planner
by Atas et al. [30] utilizes our proposed traversability mapping to generate collision-free,
cost-effective paths. The traversability map with the optimal path in yellow is shown on
the left. At the same time, the right showcases the high-fidelity simulated environment
and the corresponding robot positions for the scene.

We conducted path planning and navigation experiments to evaluate our

method in a practical scenario. Planning algorithms utilized the traversabil-
ity maps to optimize local paths by favoring regions with lower traversal costs
in these tests. We employed a variant of the A* algorithm, proposed by Atas
et al. [30], which is particularly adept at handling point clouds. Using our
traversability estimates, this algorithm refines a prior global plan within the
robot’s immediate vicinity (highlighted by the red bounding box in Fig. 17).

31

128
Figure 18: Similar to Fig. 17, the left side displays the traversability cost map with
the optimal path highlighted in yellow, and the right depicts the high-fidelity simulated
environment and the robot.

Our experiments further demonstrated an additional application of the

proposed method: exploration in the absence of a pre-existing global map
or plan. Here, we set a dynamic goal that shifts with the robot’s position,
enabling it to explore the terrain autonomously until it assesses that further
movement may result in a collision. These experiments demonstrate the
capability of the proposed method to guide a robot in choosing safe and
feasible paths within complex environments.

5. Concluding Remarks
This paper introduces a novel neural network architecture to predict con-
tinuous traversability values using high-resolution, robot-centric point cloud

32

129
 maps. Leveraging deep neural networks, our method circumvents the tedious
process of manual data labeling, as both data and labels are autonomously
generated within a high-fidelity simulation environment.
Our extensive experiments validate the robust performance of our pro-
posed method, exhibiting highly accurate predictions on our collected valida-
tion and third-party datasets. Furthermore, the pre-trained neural network
model was directly deployed on an actual robot platform, providing satis-
factory traversability estimates even without integrating any real-world data
samples.
The practicality of our approach is further demonstrated through its ap-
plication in 3D path planning algorithms that operate directly on point
clouds. In such applications, our method consistently enabled planners to
produce collision-free paths, proving critical for safe navigation and explo-
ration within complex outdoor environments.
We believe our work pioneers a new approach to handling the challenges
of data generation through realistic simulations and employing a deep neural
network tailored to interpret dense point cloud maps directly. While our
method adeptly utilizes spatiotemporal robot-centric point cloud maps, it
has limitations. Dynamic objects like pedestrians may become embedded
within the map, leading to potential prediction inaccuracies.
Looking ahead, we aim to refine our system to produce point-wise con-
tinuous traversability estimates rather than relying on a discretized robot
footprint box. This enhancement will help to preserve information integrity
and provide even more precise traversability estimates for autonomous robot
navigation.

33

130
 Statement: During the parts of the preparation of this work, the au-
thor(s) used ChatGPT and Grammarly in order to improve the language
and readability. After using this tool/service, the author(s) reviewed and
edited the content as needed and take(s) full responsibility for the content of
the publication.

References
[1] E. Wallin, V. Wiberg, F. Vesterlund, J. Holmgren, H. J.
Persson, M. Servin, Learning multiobjective rough terrain
traversability, Journal of Terramechanics 102 (2022) 17–26.
doi:https://doi.org/10.1016/j.jterra.2022.04.002.
URL https://www.sciencedirect.com/science/article/pii/
S0022489822000313

[2] K. Koide, M. Yokozuka, S. Oishi, A. Banno, Voxelized gicp for fast and
accurate 3d point cloud registration, in: 2021 IEEE International Con-
ference on Robotics and Automation (ICRA), 2021, pp. 11054–11059.
doi:10.1109/ICRA48506.2021.9560835.

[3] I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, C. Stach-

niss, KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate,
and Robust Registration If Done the Right Way, IEEE Robotics and Au-
tomation Letters (RA-L) 8 (2) (2023) 1029–1036. doi:10.1109/LRA.
2023.3236571.

[4] R. Manduchi, A. Castano, A. Talukder, L. Matthies, Obstacle de-

tection and terrain classification for autonomous off-road naviga-
tion, Autonomous Robots 18 (2005) 81–102. doi:10.1023/B:AURO.
0000047286.62481.1d.

[5] M. Bajracharya, J. Ma, M. Malchano, A. Perkins, A. A. Rizzi,

L. Matthies, High fidelity day/night stereo mapping with vegetation
and negative obstacle detection for vision-in-the-loop walking, in: 2013
IEEE/RSJ International Conference on Intelligent Robots and Systems,
2013, pp. 3663–3670. doi:10.1109/IROS.2013.6696879.

[6] A. Angelova, L. Matthies, D. Helmick, P. Perona, Fast terrain classifica-

tion using variable-length representation for autonomous navigation, in:

34

131
 2007 IEEE Conference on Computer Vision and Pattern Recognition,
2007, pp. 1–8. doi:10.1109/CVPR.2007.383024.
[7] M. Bajracharya, B. Tang, A. Howard, M. Turmon, L. Matthies, Learning
long-range terrain classification for autonomous navigation, in: 2008
IEEE International Conference on Robotics and Automation, 2008, pp.
4018–4024. doi:10.1109/ROBOT.2008.4543828.
[8] K. Shen, M. F. Kelly, Terrain classification for off-road driving cs-229
final report, 2017.
URL https://api.semanticscholar.org/CorpusID:30413317
[9] T. H. Y. Leung, D. Ignatyev, A. Zolotas, Hybrid terrain traversability
analysis in off-road environments, in: 2022 8th International Conference
on Automation, Robotics and Applications (ICARA), 2022, pp. 50–56.
doi:10.1109/ICARA55094.2022.9738557.
[10] S. Hosseinpoor, J. Torresen, M. Mantelli, D. Pitto, M. Kolberg, R. Maf-
fei, E. Prestes, Traversability analysis by semantic terrain segmenta-
tion for mobile robots, in: 2021 IEEE 17th International Conference
on Automation Science and Engineering (CASE), 2021, pp. 1407–1413.
doi:10.1109/CASE49439.2021.9551629.
[11] M. A. Bekhti, Y. Kobayashi, Regressed terrain traversability cost for
autonomous navigation based on image textures, Applied Sciences 10 (4)
(2020). doi:10.3390/app10041195.
URL https://www.mdpi.com/2076-3417/10/4/1195
[12] R. O. Chavez-Garcia, J. Guzzi, L. M. Gambardella, A. Giusti, Learning
ground traversability from simulations, CoRR abs/1709.05368 (2017).
arXiv:1709.05368.
URL http://arxiv.org/abs/1709.05368
[13] R. Agishev, T. Petřı́ček, K. Zimmermann, Trajectory optimization us-
ing learned robot-terrain interaction model in exploration of large sub-
terranean environments, IEEE Robotics and Automation Letters 7 (2)
(2022) 3365–3371. doi:10.1109/LRA.2022.3147332.
[14] G. G. Waibel, T. Löw, M. Nass, D. Howard, T. Bandyopadhyay,
P. V. K. Borges, How rough is the path? terrain traversability es-
timation for local and global path planning, IEEE Transactions on

35

132
 Intelligent Transportation Systems 23 (9) (2022) 16462–16473. doi:
10.1109/TITS.2022.3150328.

[15] F. Atas, G. Cielniak, L. Grimstad, Elevation state-space: Surfel-

based navigation in uneven environments for mobile robots, in: 2022
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 2022, pp. 5715–5721. doi:10.1109/IROS47612.2022.9981647.

[16] J. L. Martı́nez, M. Morán, J. Morales, A. Robles, M. Sánchez, Super-

vised learning of natural-terrain traversability with synthetic 3d laser
scans, Applied Sciences 10 (3) (2020). doi:10.3390/app10031140.
URL https://www.mdpi.com/2076-3417/10/3/1140

[17] R. Thakker, N. Alatur, D. D. Fan, J. Tordesillas, M. Paton, K. Otsu,

O. Toupet, A.-a. Agha-mohammadi, Autonomous off-road navigation
over extreme terrains with perceptually-challenging conditions, in: B. Si-
ciliano, C. Laschi, O. Khatib (Eds.), Experimental Robotics, Springer
International Publishing, Cham, 2021, pp. 161–173.

[18] H. Xue, H. Fu, L. Xiao, Y. Fan, D. Zhao, B. Dai, Traversability anal-

ysis for autonomous driving in complex environment: A LiDAR-based
terrain modeling approach, Journal of Field Robotics 40 (7) (2023) 1779–
1803. doi:10.1002/rob.22209.
URL https://doi.org/10.1002%2Frob.22209

[19] D. Fusaro, E. Olivastri, D. Evangelista, M. Imperoli, E. Menegatti,

A. Pretto, Pushing the limits of learning-based traversability analysis
for autonomous driving on cpu, in: I. Petrovic, E. Menegatti, I. Marković
(Eds.), Intelligent Autonomous Systems 17, Springer Nature Switzer-
land, Cham, 2023, pp. 529–545.

[20] K. Zhang, Y. Yang, M. Fu, M. Wang, Traversability assessment and tra-

jectory planning of unmanned ground vehicles with suspension systems
on rough terrain, Sensors 19 (20) (2019). doi:10.3390/s19204372.
URL https://www.mdpi.com/1424-8220/19/20/4372

[21] G. Haddeler, M. Y. M. Chuah, Y. You, J. Chan, A. H. Adiwahono,

W. Y. Yau, C.-M. Chew, Traversability analysis with vision and terrain
probing for safe legged robot navigation, Frontiers in Robotics and AI
9 (2022). doi:10.3389/frobt.2022.887910.

36

133
 URL https://www.frontiersin.org/articles/10.3389/frobt.
2022.887910
[22] T. Guan, Z. He, D. Manocha, L. Zhang, Ttm: Terrain traversability
mapping for autonomous excavator navigation in unstructured environ-
ments, ArXiv abs/2109.06250 (2021).
URL https://api.semanticscholar.org/CorpusID:237503221
[23] P. Borges, T. Peynot, S. Liang, B. Arain, M. Wildie, M. Minareci,
S. Lichman, G. Samvedi, I. Sa, N. Hudson, et al., A survey on ter-
rain traversability analysis for autonomous ground vehicles: Methods,
sensors, and challenges, Field Robot 2 (1) (2022) 1567–1627.
[24] C. R. Qi, H. Su, K. Mo, L. J. Guibas, Pointnet: Deep learning
on point sets for 3d classification and segmentation, arXiv preprint
arXiv:1612.00593 (2016).
[25] S. Shi, X. Wang, H. Li, Pointrcnn: 3d object proposal generation and
detection from point cloud, in: The IEEE Conference on Computer
Vision and Pattern Recognition (CVPR), 2019.
[26] C. R. Qi, L. Yi, H. Su, L. J. Guibas, Pointnet++: Deep hierarchi-
cal feature learning on point sets in a metric space, arXiv preprint
arXiv:1706.02413 (2017).
[27] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs,
R. Wheeler, A. Ng, Ros: an open-source robot operating system, Vol. 3,
2009.
[28] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, R. Daniela, Lio-sam:
Tightly-coupled lidar inertial odometry via smoothing and mapping, in:
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), IEEE, 2020, pp. 5135–5142.
[29] D. P. Kingma, J. Ba, Adam: A method for stochastic optimization,
CoRR abs/1412.6980 (2014).
URL https://api.semanticscholar.org/CorpusID:6628106
[30] F. Atas, G. Cielniak, L. Grimstad, Navigating in 3d uneven environ-
ments through supervoxels and nonlinear mpc, in: 2023 European Con-
ference on Mobile Robots (ECMR), 2023, pp. 1–8. doi:10.1109/
ECMR59166.2023.10256342.

37

134
Paper VI

Enabling Robot Autonomy through

a Modular Software Framework

Fetullah Atas, Grzegorz Cielniak, Lars Grimstad

CRA Workshop on Robotic Software Architectures, 2023.
https://doi.org/10.1002/rob.21878.

VI
135
Enabling Robot Autonomy through a Modular Software Framework
Fetullah Atas 1 Grzegorz Cielniak 2 Lars Grimstad 1

Abstract—The complexity of robotic software architectures

stems from the need to manage a diverse range of sensory inputs,
real-time actuator control, and adaptive capabilities in dynamic
environments. In order to guarantee safe operation, robots must
be capable of executing tasks concurrently and asynchronously,
which poses significant challenges in developing cohesive robotic
software architectures.
It is commonly accepted that there is no universal approach
that can address the needs of all robot platforms and applications.
A number of established architectures have been developed based
on the publish-subscribe and action-client paradigms employed
by Robot Operating System (ROS) middleware. Extending
on these developments, in this research, we present a novel
robotic software architecture that enables seamless integration
of different robotics software components, such as Planning, Fig. 1: In this illustration, we see various components that have
Control, and Perception. The presented architecture is designed
to ensure the autonomous navigation of a mobile robot operating
been integrated into our robot navigation framework. Each
in uneven outdoor terrains, while also supporting indoor sub-component (e.g., planning, localization, etc.) is linked
environments with appropriate customization. Our software has to standard libraries; for instance, motion planning relies
been made available to the robotics community through a GitHub on the OMPL library, and localization uses the Extended
repository.1 . Kalman Filter (robot localization). The core servers utilize
Index Terms—Uneven Terrain Navigation, Motion Planning,
these libraries to load and operate specific plugins, these
Navigation Framework
plugins are then exposed as actions and can be used to
define various robot behaviors such navigate to pose, navigate
I. I NTRODUCTION
through poses, etc. using a behavior tree approach.
The use of robots in large outdoor areas with uneven
terrain is becoming more prevalent, with applications ranging
from delivery robots and plenary discovery robots to legged
robots that navigate unstructured terrain and mobile robots map or a full 3D occupancy grid by simply redefining a
that perform agricultural tasks in open fields. However, robots function that checks for collisions between the robot and
operating in uneven environments face challenges due to the environment at a given state. This feature provides
changes in elevation and the unstructured nature of the flexibility in selecting the most appropriate representation for
environment, popular 2D occupancy grids may not accurately a given application, without compromising on the planning
represent the space of operation in such cases. Among the and navigation performance of the robot, unlike previously
software architectures used for robot navigation, Navigation proposed navigation frameworks. The proposed software
2 [6] and its predecessor, the original ROS [10] navigation architecture is influenced by Navigation 2 [6], however, the
framework [7], are popular in academia and industry. These proposed method is more flexible in terms of switching
methods rely on 2D occupancy grids to represent the different components depending on the task at hand, these
environment and have been widely adopted by researchers. could include switching planners, controllers, or environment
Other methods, such as [9], [11], use 3D meshes generated representation. All components are implemented as plugins
from 3D point cloud maps to represent unstructured terrains. that use the polymorphism paradigm of object-oriented
Our research proposes a novel navigation architecture programming.
for robots in uneven terrains based on ROS 2 [5].
Our architecture leverages dozens of state-of-the-art motion To summarize, our approach presents the following key
planning algorithms from OMPL [13], which can handle contributions:
robots operating in different state spaces and with varying
kinematic and dynamic constraints. The abstract nature of • A ROS 2-enabled modular interface that can be
the OMPL library makes it possible to switch between customized without modifying the code base to the
different environment representations, such as a 2.5D elevation specific needs of different robots.
1 Norwegian University of Life Sciences {fetullah.atas,
• The method enables users to switch environment
lars.grimstad} @nmbu.no
representation without the need to adapt motion planners,
2 University of Lincoln gcielniak@lincoln.ac.uk but rather a single C++ function, namely
1 https://github.com/NMBURobotics/vox nav bool isValid(ompl::base::State *state).

137
 II. A PPROACH
The essential elements of the proposed method are presented
in Fig. 2. The fundamental external libraries employed in
our framework include OMPL [13], robot localization [8],
LIO-SAM [12], and Casadi [1]. The three server nodes,
namely map server, planner server, and control server, offer
a plugin interface. This strategy facilitates the integration
of numerous planning and control plugins into the system, Fig. 2: The figure showcases the range of terrains on which
without necessitating any code modification. Instead, a YAML the framework was evaluated. Specifically, the left image
configuration file can be employed for different plugin depicts a robot operating on snowy, uneven terrain, the middle
integration. image shows a robot operating in an open field, and the right
image demonstrates a robot autonomously navigating in the
A. Plugin Specifications strawberry polytunnel.
At the time of writing, the system incorporates 2 map
plugins, 3 planner plugins, and 3 controller plugins. To
illustrate how our approach can be tailored to specific instance, the Acado implementation boasts an exceptionally
requirements, we will focus on one plugin from each of the fast control step with kHz frequency updates. In contrast, the
three categories. Casadi implementation generates smoother trajectories at low
1) Point Cloud Map Plugin: For an outdoor robot operating speeds, demonstrating the flexibility of the approach.
in uneven terrains, a 3D point cloud map serves as an effective
tool for representing the environment [2]. The objective of this III. E XPERIMENTS
plugin is to furnish a map that incorporates semantic data, such
The proposed framework was evaluated in both simulated
as whether a particular region of the environment is traversable
and real-world settings, using an agricultural robot platform
or not. The plugin offers the map and semantic data to theknown as Thorvald II [3]. In comparison to Navigation
planner or controller plugins through ROS servers and clients.
2 [6], a widely used navigation framework that is built
2) Planner Plugin: The planner plugins are classified based
on top of a well-established ROS 1 navigation stack [7],
on the state space in which the robot operates, such as SE2
our approach demonstrates superior performance in modeling
or SE3. Various parameters can be specified for the plugins,
the unstructured nature of outdoor scenes, especially in
including the collision body dimensions of the robot, state
terrains with steep inclinations. While Navigation2 is primarily
boundaries, and actual planning algorithms. For example, adesigned for indoor environments, it can also be applied to
configuration for a mobile robot operating in flat terrain may
outdoor scenes. Notably, both frameworks exhibit satisfactory
include the following specifications: performance in even terrain patches, but our framework
planner server node: outperforms Navigation2 in uneven terrains.
planner plugin: "SE2Planner" # or "SE3Planner", etc.
expected planner frequency: 1.0
The present approach was also tested for polytunnel
planning algorithm: "RRTStar" # or other OMPL planners navigation for an agricultural robot. Navigation within
planner timeout: 35.0 polytunnels poses a considerable challenge due to the absence
robot body dimens:
x: 1.2
of GPS signals, necessitating precise localization accuracy to
y: 1.2 prevent the robot from colliding with fruit rows. With the
z: 0.8 aid of a point cloud map, the proposed framework permits
SE2Planner:
plugin: "vox_nav_planning::SE2Planner"
LIDAR-based localizers to employ the Iterative Closest Point
se2 space: "REEDS" # or "DUBINS" (ICP) and Normal Distribution Transform (NDT) point cloud
rho: 2.5 # Curve radius for reeds and dubins spaces matching algorithms to enhance localization accuracy.
state space boundries:
minx: -100.0
maxx: 100.0 IV. C ONCLUSION
miny: -100.0
maxy: 100.0 We have presented a new software framework for robots
minyaw: -3.14 that can easily adapt to various environments and robot
maxyaw: 3.14
types. Unlike existing frameworks, our approach allows for
modifying the environment representation without requiring
3) Controller Plugin: The proposed framework is designed any adjustments to motion planners. Therefore, the proposed
specifically for mobile robots, and thus, the available controller framework is suitable for mobile robots, UAVs, and other
plugins are model-based controllers, such as MPC, tailored robots that operate under diverse kinematic constraints.
for mobile robot control. To ensure versatility, we have Our framework has been evaluated in both simulated and
incorporated two different MPC controller implementations real-world settings. However, further research is needed
using distinct optimization libraries, namely Casadi [1] and to enhance its reliability, particularly in dynamic urban
Acado [4]. Each implementation offers unique advantages. For environments.

138
 R EFERENCES
[1] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl.
CasADi – A software framework for nonlinear optimization and optimal
control. Mathematical Programming Computation, 11(1):1–36, 2019.
[2] F. Atas, G. Cielniak, and L. Grimstad. Elevation state-space:
Surfel-based navigation in uneven environments for mobile robots. In
To appear in 2022 IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), 2022.
[3] L. Grimstad and P. J. From. Thorvald ii - a modular and re-configurable
agricultural robot. IFAC-PapersOnLine, 50(1):4588–4593, 2017. 20th
IFAC World Congress.
[4] B. Houska, H. Ferreau, and M. Diehl. An Auto-Generated Real-Time
Iteration Algorithm for Nonlinear MPC in the Microsecond Range.
Automatica, 47(10):2279–2285, 2011.
[5] S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall. Robot
operating system 2: Design, architecture, and uses in the wild. Science
Robotics, 7(66):eabm6074, 2022.
[6] S. Macenski, F. Martin, R. White, and J. Ginés Clavero. The marathon
2: A navigation system. In 2020 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS), 2020.
[7] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, and K. Konolige.
The office marathon: Robust navigation in an indoor office environment.
In International Conference on Robotics and Automation, 2010.
[8] T. Moore and D. Stouch. A generalized extended kalman filter
implementation for the robot operating system. In E. Menegatti,
N. Michael, K. Berns, and H. Yamaguchi, editors, Intelligent
Autonomous Systems 13, pages 335–348, Cham, 2016. Springer
International Publishing.
[9] S. Pütz, T. Wiemann, J. Sprickerhof, and J. Hertzberg. 3d
navigation mesh generation for path planning in uneven terrain.
IFAC-PapersOnLine, 49(15):212–217, 2016. 9th IFAC Symposium on
Intelligent Autonomous Vehicles IAV 2016.
[10] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs,
R. Wheeler, and A. Ng. Ros: an open-source robot operating system.
volume 3, 01 2009.
[11] F. Ruetz, E. Hernández, M. Pfeiffer, H. Oleynikova, M. Cox, T. Lowe,
and P. Borges. Ovpc mesh: 3d free-space representation for local ground
vehicle navigation. In 2019 International Conference on Robotics and
Automation (ICRA), pages 8648–8654, 2019.
[12] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela.
Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and
mapping. In IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), pages 5135–5142. IEEE, 2020.
[13] I. A. Sucan, M. Moll, and L. E. Kavraki. The open motion planning
library. IEEE Robotics Automation Magazine, 19(4):72–82, 2012.

139
Bibliography
[1] L. Grimstad and P. J. From, “Thorvald ii - a modular and re-
configurable agricultural robot,” IFAC-PapersOnLine, vol. 50, no. 1,
pp. 4588–4593, 2017, 20th IFAC World Congress. [Online]. Available:
https://www.sciencedirect.com/science/article/pii/S2405896317314830

[2] L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, “Probabilistic

roadmaps for path planning in high-dimensional configuration spaces,” IEEE
Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.

[3] T. Shan, J. Wang, B. Englot, and K. Doherty, “Bayesian generalized

kernel inference for terrain traversability mapping,” in Proceedings of
The 2nd Conference on Robot Learning, ser. Proceedings of Machine
Learning Research, A. Billard, A. Dragan, J. Peters, and J. Morimoto,
Eds., vol. 87. PMLR, 29–31 Oct 2018, pp. 829–838. [Online]. Available:
https://proceedings.mlr.press/v87/shan18a.html

[4] R. O. Chavez-Garcia, J. Guzzi, L. M. Gambardella, and A. Giusti,

“Image classification for ground traversability estimation in robotics,” in
Advanced Concepts for Intelligent Vision Systems Conference, 2017. [Online].
Available: https://api.semanticscholar.org/CorpusID:24540986

[5] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, and K. Konolige, “The

office marathon: Robust navigation in an indoor office environment,” in
International Conference on Robotics and Automation, 2010.

[6] L. E. Dubins, “On curves of minimal length with a constraint on average

curvature, and with prescribed initial and terminal positions and tangents,”
American Journal of Mathematics, vol. 79, no. 3, pp. 497–516, 1957.
[Online]. Available: http://www.jstor.org/stable/2372560

[7] J. A. Reeds and L. A. Shepp, “Optimal paths for a car that goes both
forwards and backwards.” Pacific Journal of Mathematics, vol. 145, no. 2,
pp. 367 – 393, 1990. [Online]. Available: https://doi.org/

[8] D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to

collision avoidance,” Robotics and Automation Magazine, IEEE, vol. 4, pp.
23 – 33, 04 1997.

[9] S. Lavalle and J. Kuffner, “Rapidly-exploring random trees: Progress and

prospects,” Algorithmic and computational robotics: New directions, 01
2000.

141
Bibliography

[10] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal

motion planning,” CoRR, vol. abs/1105.1186, 2011. [Online]. Available:
http://arxiv.org/abs/1105.1186
[11] I. A. Sucan, M. Moll, and L. E. Kavraki, “The open motion planning library,”
IEEE Robotics Automation Magazine, vol. 19, no. 4, pp. 72–82, 2012.
[12] J. D. Gammell, T. D. Barfoot, and S. S. Srinivasa, “Batch informed trees
(bit*): Informed asymptotically optimal anytime search,” The International
Journal of Robotics Research, vol. 39, no. 5, pp. 543–567, 2020. [Online].
Available: https://doi.org/10.1177/0278364919890396
[13] M. P. Strub and J. D. Gammell, “Advanced bit* (abit*): Sampling-based
planning with advanced graph-search techniques,” 2020 IEEE International
Conference on Robotics and Automation (ICRA), May 2020. [Online].
Available: http://dx.doi.org/10.1109/ICRA40945.2020.9196580
[14] ——, “Adaptively Informed Trees (AIT*): Fast asymptotically optimal
path planning through adaptive heuristics,” in Proceedings of the IEEE
International Conference on Robotics and Automation (ICRA), 31 May –
31 Aug. 2020, pp. 3191–3198.
[15] M. Otte and N. Correll, “C-forest: Parallel shortest path planning with
superlinear speedup,” IEEE Transactions on Robotics, vol. 29, no. 3, pp.
798–806, 2013.
[16] R. Luna, I. A. Şucan, M. Moll, and L. E. Kavraki, “Anytime solution opti-
mization for sampling-based motion planning,” in 2013 IEEE International
Conference on Robotics and Automation, 2013, pp. 5068–5074.
[17] S. M. LaValle and J. James J. Kuffner, “Randomized kinodynamic planning,”
The International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400,
2001. [Online]. Available: https://doi.org/10.1177/02783640122067453
[18] D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive
configuration spaces,” in Proceedings of International Conference on Robotics
and Automation, vol. 3, 1997, pp. 2719–2726 vol.3.
[19] Y. Li, Z. Littlefield, and K. E. Bekris, “Asymptotically optimal
sampling-based kinodynamic planning,” CoRR, vol. abs/1407.2896, 2014.
[Online]. Available: http://arxiv.org/abs/1407.2896
[20] K. Hauser and Y. Zhou, “Asymptotically optimal planning by feasible
kinodynamic planning in a state–cost space,” IEEE Transactions on
Robotics, vol. 32, no. 6, pp. 1431–1443, 2016.
[21] Z. Littlefield and K. E. Bekris, “Efficient and asymptotically optimal
kinodynamic motion planning via dominance-informed regions,” in 2018
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 2018, pp. 1–9.

142
 Bibliography

[22] M. Kleinbort, K. Solovey, Z. Littlefield, K. Bekris, and D. Halperin,

“Probabilistic completeness of rrt for geometric and kinodynamic planning
with forward propagation,” IEEE Robotics and Automation Letters, vol. PP,
pp. 1–1, 12 2018.

[23] M. Kleinbort, K. Solovey, R. Bonalli, K. E. Bekris, and D. Halperin,

“RRT2.0 for fast and optimal kinodynamic sampling-based motion
planning,” CoRR, vol. abs/1909.05569, 2019. [Online]. Available:
http://arxiv.org/abs/1909.05569

[24] R. Shome and L. E. Kavraki, “Asymptotically optimal kinodynamic planning

using bundles of edges,” in 2021 IEEE International Conference on Robotics
and Automation (ICRA), 2021, pp. 9988–9994.

[25] S. Macenski, F. Martin, R. White, and J. Ginés Clavero, “The marathon

2: A navigation system,” in 2020 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS), 2020.

[26] M. Wermelinger, P. Fankhauser, R. Diethelm, P. Krüsi, R. Siegwart, and

M. Hutter, “Navigation planning for legged robots in challenging terrain,”
10 2016, pp. 1184–1189.

[27] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss,

and W. Burgard, “OctoMap: An efficient probabilistic 3D
mapping framework based on octrees,” Autonomous Robots, 2013,
software available at http://octomap.github.com. [Online]. Available:
http://octomap.github.com

[28] C. Wang, L. Meng, S. She, I. M. Mitchell, T. Li, F. Tung, W. Wan, M. Q.-H.

Meng, and C. W. de Silva, “Autonomous mobile robot navigation in uneven
and unstructured indoor environments,” in 2017 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2017, pp. 109–116.

[29] S. Pütz, T. Wiemann, J. Sprickerhof, and J. Hertzberg, “3d

navigation mesh generation for path planning in uneven terrain,” IFAC-
PapersOnLine, vol. 49, no. 15, pp. 212–217, 2016, 9th IFAC Symposium
on Intelligent Autonomous Vehicles IAV 2016. [Online]. Available:
https://www.sciencedirect.com/science/article/pii/S2405896316310102

[30] F. Ruetz, E. Hernández, M. Pfeiffer, H. Oleynikova, M. Cox, T. Lowe, and

P. Borges, “Ovpc mesh: 3d free-space representation for local ground vehicle
navigation,” in 2019 International Conference on Robotics and Automation
(ICRA), 2019, pp. 8648–8654.

[31] R. Manduchi, A. Castano, A. Talukder, and L. Matthies, “Obstacle detection

and terrain classification for autonomous off-road navigation,” Autonomous
Robots, vol. 18, pp. 81–102, 01 2005.

143
Bibliography

[32] M. Bajracharya, J. Ma, M. Malchano, A. Perkins, A. A. Rizzi, and

L. Matthies, “High fidelity day/night stereo mapping with vegetation
and negative obstacle detection for vision-in-the-loop walking,” in 2013
IEEE/RSJ International Conference on Intelligent Robots and Systems,
2013, pp. 3663–3670.

[33] A. Angelova, L. Matthies, D. Helmick, and P. Perona, “Fast terrain classifi-

cation using variable-length representation for autonomous navigation,” in
2007 IEEE Conference on Computer Vision and Pattern Recognition, 2007,
pp. 1–8.

[34] M. Bajracharya, B. Tang, A. Howard, M. Turmon, and L. Matthies,

“Learning long-range terrain classification for autonomous navigation,” in
2008 IEEE International Conference on Robotics and Automation, 2008, pp.
4018–4024.
[35] K. Shen and M. F. Kelly, “Terrain classification for off-
road driving cs-229 final report,” 2017. [Online]. Available:
https://api.semanticscholar.org/CorpusID:30413317
[36] T. H. Y. Leung, D. Ignatyev, and A. Zolotas, “Hybrid terrain traversability
analysis in off-road environments,” in 2022 8th International Conference on
Automation, Robotics and Applications (ICARA), 2022, pp. 50–56.

[37] S. Hosseinpoor, J. Torresen, M. Mantelli, D. Pitto, M. Kolberg, R. Maffei,

and E. Prestes, “Traversability analysis by semantic terrain segmentation for
mobile robots,” in 2021 IEEE 17th International Conference on Automation
Science and Engineering (CASE), 2021, pp. 1407–1413.
[38] R. Agishev, T. Petříček, and K. Zimmermann, “Trajectory optimization
using learned robot-terrain interaction model in exploration of large
subterranean environments,” IEEE Robotics and Automation Letters, vol. 7,
no. 2, pp. 3365–3371, 2022.
[39] G. G. Waibel, T. Löw, M. Nass, D. Howard, T. Bandyopadhyay, and P. V. K.
Borges, “How rough is the path? terrain traversability estimation for local
and global path planning,” IEEE Transactions on Intelligent Transportation
Systems, vol. 23, no. 9, pp. 16 462–16 473, 2022.
[40] J. L. Martínez, M. Morán, J. Morales, A. Robles, and M. Sánchez,
“Supervised learning of natural-terrain traversability with synthetic 3d
laser scans,” Applied Sciences, vol. 10, no. 3, 2020. [Online]. Available:
https://www.mdpi.com/2076-3417/10/3/1140

[41] R. Thakker, N. Alatur, D. D. Fan, J. Tordesillas, M. Paton, K. Otsu,

O. Toupet, and A.-a. Agha-mohammadi, “Autonomous off-road navigation
over extreme terrains with perceptually-challenging conditions,” in Exper-
imental Robotics, B. Siciliano, C. Laschi, and O. Khatib, Eds. Cham:
Springer International Publishing, 2021, pp. 161–173.

144
 Bibliography

[42] E. Wallin, V. Wiberg, F. Vesterlund, J. Holmgren, H. J. Persson,

and M. Servin, “Learning multiobjective rough terrain traversability,”
Journal of Terramechanics, vol. 102, pp. 17–26, 2022. [Online]. Available:
https://www.sciencedirect.com/science/article/pii/S0022489822000313

[43] D. Fusaro, E. Olivastri, D. Evangelista, M. Imperoli, E. Menegatti, and

A. Pretto, “Pushing the limits of learning-based traversability analysis
for autonomous driving on cpu,” in Intelligent Autonomous Systems 17,
I. Petrovic, E. Menegatti, and I. Marković, Eds. Cham: Springer Nature
Switzerland, 2023, pp. 529–545.

[44] K. Zhang, Y. Yang, M. Fu, and M. Wang, “Traversability assessment and

trajectory planning of unmanned ground vehicles with suspension systems
on rough terrain,” Sensors, vol. 19, no. 20, 2019. [Online]. Available:
https://www.mdpi.com/1424-8220/19/20/4372

[45] G. Haddeler, M. Y. M. Chuah, Y. You, J. Chan, A. H.

Adiwahono, W. Y. Yau, and C.-M. Chew, “Traversability analysis
with vision and terrain probing for safe legged robot navigation,”
Frontiers in Robotics and AI, vol. 9, 2022. [Online]. Available:
https://www.frontiersin.org/articles/10.3389/frobt.2022.887910

[46] T. Guan, Z. He, D. Manocha, and L. Zhang, “Ttm: Terrain

traversability mapping for autonomous excavator navigation in unstructured
environments,” ArXiv, vol. abs/2109.06250, 2021. [Online]. Available:
https://api.semanticscholar.org/CorpusID:237503221

[47] P. Borges, T. Peynot, S. Liang, B. Arain, M. Wildie, M. Minareci,

S. Lichman, G. Samvedi, I. Sa, N. Hudson et al., “A survey on terrain
traversability analysis for autonomous ground vehicles: Methods, sensors,
and challenges,” Field Robot, vol. 2, no. 1, pp. 1567–1627, 2022.

[48] Z. Jian, Z. Lu, X. Zhou, B. Lan, A. Xiao, X. Wang, and B. Liang,

“Putn: A plane-fitting based uneven terrain navigation framework,” in 2022
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 2022, pp. 7160–7166.

[49] X. Tian, S. Yang, K. Yu, M. Fu, and W. Song, “An efficient planning
framework for ground vehicles navigating on uneven terrain,” in 2023 IEEE
International Conference on Unmanned Systems (ICUS), 2023, pp. 1493–
1498.

[50] H. Lee, J. Kwon, and C. Kwon, “Learning-based uncertainty-aware

navigation in 3d off-road terrains,” in 2023 IEEE International Conference
on Robotics and Automation (ICRA), 2023, pp. 10 061–10 068.

[51] J. Seo, J. Mun, and T. Kim, “Safe navigation in unstructured environments

by minimizing uncertainty in control and perception,” 2023.

145
Bibliography

[52] X. Xiao, B. Liu, G. Warnell, and P. Stone, “Motion control for mobile robot
navigation using machine learning: a survey,” CoRR, vol. abs/2011.13112,
2020. [Online]. Available: https://arxiv.org/abs/2011.13112
[53] M. Bakken, “Explainable and data-efficient learning for visual guidance of
autonomous agri-robots,” PhD thesis, Norwegian University of Life Sciences,
As, Norway, July 2021, available at https://hdl.handle.net/11250/2788278.
[54] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-
source multi-robot simulator,” in 2004 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566),
vol. 3, 2004, pp. 2149–2154 vol.3.
[55] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler,
and A. Ng, “Ros: an open-source robot operating system,” vol. 3, 01 2009.
[56] M. Aicardi, G. Casalino, A. Bicchi, and A. Balestrino, “Closed loop steering
of unicycle like vehicles via lyapunov techniques,” IEEE Robotics Automation
Magazine, vol. 2, no. 1, pp. 27–35, 1995.

146
ISBN: 978-82-575-2145-5
ISSN: 1894-6402

120393 / MEDIEHUSET-ANDVORD.NO

Postboks 5003
NO-1432 Ås, Norway
+47 67 23 00 00
www.nmbu.no