Available online at www.sciencedirect.

com

ScienceDirect
IFAC PapersOnLine 55-38 (2022) 80–85
2.5D
2.5D Mapping,
Mapping, Pathfinding
Pathfinding and
and Path
Path
2.5D
2.5D Mapping,
Mapping, Pathfinding
Pathfinding and
and Path
Path
Following
2.5D
Following
2.5D For
Mapping,
For
Mapping,Navigation
Pathfinding
Navigation
Pathfinding Of
Of A
A Differential
and Path
Differential
and Path
Following
Following For
For Navigation
Navigation Of
Of A
A Differential
Differential

Drive
Following
Drive
Following Robot
For
Robot
For In
In Uneven
Navigation
Uneven
Navigation Of
Of ATerrain
A Differential
Terrain 
Differential
Drive
Drive Robot
Robot In
In Uneven
Uneven Terrain
Terrain
Drive
Drive Robot
Robot In Uneven
In ∗,∗∗
Uneven Terrain
Terrain ∗ 
Stepan
Stepan Dergachev
∗,∗∗
Dergachev ∗,∗∗ Kirill Muravyev ∗
∗
∗,∗∗ Kirill Muravyev ∗
Stepan Dergachev
Dergachev ∗,∗∗
Stepan Konstantin Kirill
Kirill Muravyev
Yakovlev
∗,∗∗ ∗,∗∗
Muravyev
∗,∗∗ ∗
∗
Stepan Konstantin Yakovlev ∗,∗∗
Dergachev ∗,∗∗
Konstantin Kirill Muravyev
Yakovlev
∗,∗∗
∗,∗∗
∗,∗∗
∗,∗∗
∗
∗
Stepan Konstantin
Dergachev
Konstantin Kirill
Yakovlev
Yakovlev Muravyev
∗,∗∗
∗
∗ Federal Research
∗ Federal ResearchKonstantin
∗
Center “Computer
Center “Computer Yakovlev Sciences
Sciences and Control” of
∗,∗∗ and Control” of
∗ Federal
Russian
∗ Federal Research
Academy
Research of Center
Sciences,
Center “Computer
Moscow,
“Computer Sciences
Russia
Sciences and Control”
Control” of
(e-mail:{dergachev,
and of
Russian
∗ Academy of Sciences, Moscow, Russia (e-mail:{dergachev,
∗ Federal
Russian
Russian
Research
Academy
Academy of Center
Sciences,
muraviev,
of Sciences,
“Computer
Moscow,
Moscow,
Sciences
Russia
yakovlev}@isa.ru).
Russia
and Control” of
(e-mail:{dergachev,
(e-mail:{dergachev,
Federal Research
Russian Center
muraviev, “Computer Sciences
yakovlev}@isa.ru). and Control” of
∗∗
∗∗ NationalAcademy
∗∗Russian
∗∗ NationalAcademy
Researchof
Research of
Sciences,
muraviev,
University Moscow, Russia
yakovlev}@isa.ru).
Higher School
Sciences,yakovlev}@isa.ru).
University
muraviev, Moscow,
Higher Russia
School of (e-mail:{dergachev,
of Economics, Moscow,
(e-mail:{dergachev,
Economics, Moscow,
muraviev,
∗∗ National Research University Russia
∗∗
yakovlev}@isa.ru).
Higher School of Economics, Moscow,
∗∗ National Researchmuraviev, University yakovlev}@isa.ru).
Russia
Higher
∗∗ National Research University Russia Higher School of Economics, Moscow,
Russia
School of Economics, Moscow,
National Research University Russia Higher School of Economics, Moscow,
Abstract:
Abstract: Safe Safe navigation
navigation in in uneven
uneven terrains Russia
terrains is
is an
an important
important problem problem in in robotic
robotic research.
research. In In
Abstract:
this paper Safe
we navigation
propose aa 2.5D innavigation
uneven terrains
system iswhich
an important
consists problem
of elevation in robotic
map research.
building, In
path
this paper
Abstract:
Abstract: we
Safe
Safepropose
navigation
navigation 2.5D in navigation
uneven
innavigation
uneven system
terrains
terrains is which
an
iswhich consists
important
an important of elevation
problem
problem in map
robotic
in robotic building,
research.
research. path
In
In
this paper
this paperand
planning we
we propose
local
propose path a following
a 2.5D
2.5D with
navigation systemavoidance.
obstacle
system which consists
For
consists of
local
of elevation
path
elevation map building,
following
map building,
we use path
Model
path
Abstract:
planning
this paper and Safe
we local navigation
propose path a 2.5D in
following uneven
with
navigation terrains
obstacle
system is an
whichimportant
avoidance. For
consists problem
local
of path
elevation in robotic
following
map we research.
use
building, ModelIn
path
planning
Predictive
planning and
andPathlocal
local path
Integral following
(MPPI)
patha following with
control
with obstacle
method.
obstacle avoidance.
We
avoidance. For
propose local
novel
For local path following
cost-functions
path following we
wefor use
MPPI
use Model
Modelin
this paper
Predictive
planning andwe
Path propose
localIntegral
path 2.5D
(MPPI)
following navigation
control
with system
method.
obstacle which
We
avoidance. consists
propose
For of
novel
local elevation map
cost-functions building,
for MPPI path
in
Predictive
order to Path
adapt itIntegral
to (MPPI)
elevation mapscontrol
and method.
motion We propose
through novelpath
unevenness. Wefollowing
cost-functions
evaluate wefor use
our MPPIModel
system in
planning
order to
Predictive
Predictive and
adapt
Path
Pathlocal
it path
to
Integral
Integral following
elevation
(MPPI)
(MPPI) mapswith
control
control obstacle
and motion
method.
method. avoidance.
through
We
We For
propose
propose local
unevenness.
novel
novel path Wefollowing
evaluate
cost-functions
cost-functions we use
our
for
for Model
system
MPPI
MPPI in
in
order
on to adapt
multiple it to elevation
synthetic tests and maps
in a and motion
simulated through unevenness.
environment with different Wetypes
evaluate
of our system
obstacles and
Predictive
on multiple
order
order to
to Path
adapt
adapt it
itIntegral
synthetic to
to tests (MPPI)
elevation
elevation and in
maps
mapscontrol
a and
and method.
simulated
motion
motion We
environment
through
throughproposewith novel
unevenness.
unevenness. cost-functions
different We
We types of
evaluate
evaluate for
our
our MPPI
obstacles system
system in
and
on multiple
multiple
rough
on surfaces.synthetic tests
synthetic tests and and in in aa simulated
simulated environment
environment with with different
different typestypes of of obstacles
obstacles and and
order
rough
on to adapt
surfaces.
multiple it to elevation maps and motion through unevenness.
synthetic tests and in a simulated environment with different types of obstacles and We evaluate our system
rough surfaces.
rough
Copyright
on surfaces.
multiple
rough ©synthetic
surfaces. 2022 The Authors.
tests and an open access
in ais simulated
This article under
environment with CC BY-NC-ND
the different types license
of obstacles and
(https://creativecommons.org
rough
Keywords:
Keywords: surfaces.
Mobile
Mobile Robots,
Robots, 2.5D /licenses/by-nc-nd/4.0/)
2.5D Navigation,
Navigation, 2.5D 2.5D Mapping,
Mapping, Path Path Planning,
Planning, UnevenUneven Terrain
Terrain
Keywords:
Keywords: Mobile Mobile Robots,
Robots, 2.5D 2.5D Navigation,
Navigation, 2.5D 2.5D Mapping,
Mapping, Path Path Planning,
Planning, UnevenUneven Terrain
Terrain
Keywords: Mobile Robots, 2.5D Navigation, 2.5D Mapping, Path Planning, Uneven Terrain
Keywords:1. Mobile Robots, 2.5D Navigation, 2.5DIn
1. INTRODUCTION
INTRODUCTION InMapping,
most Path Planning,
most navigation
navigation systems,Uneven
systems, robot
robot is isTerrain
moved
moved along
along the the path
path
1. INTRODUCTION In most navigation
planned by global systems,
path robotinisamoved
planner global along
map. the
In path
case
1. INTRODUCTION
1. INTRODUCTION planned
In
In most by global
navigation
most navigation path
systems,
systems, planner
robot
robot in is a global
moved
isamoved map.
along
along In
the
the case
path
path
Autonomous navigation
navigation of aa mobile
mobile robot robot is is an
an important
important In planned
of uneven
planned by
by global
terrains
global path
or
path planner
dynamic
planner in
objects,
in a global
there
global map.
may
map. In
be
In case
small
case
Autonomous 1. INTRODUCTION
of of most
uneven
planned navigation
terrains
byterrains
global or orsystems,
path dynamic robot
planner is
objects, moved
there
in astones,
global along
may
map.be the
be path
small
Insmall
case
Autonomous
problem for navigation
both of a mobile
practitioners and robot is an important
researchers. A typical of uneven
unmapped
of uneven obstacles
terrains or dynamic
(pits,
dynamic objects,
hillocks,
objects, there
there may
garbage
may be etc.)
small
problem
Autonomous
Autonomous for both practitioners
navigation
navigation of and researchers.
of aa mobile
mobile robot is
robot is an A typical planned
an important
important unmapped
of uneven
unmapped
byterrains
global or
obstacles
obstacles
path planner
(pits,
dynamic
(pits,
in astones,
hillocks,
objects,
hillocks,
global
there
stones,
map.be
garbage
may
garbage
Insmall
case
etc.)
etc.)
problem
approach
problem for
for both
to navigation
both practitioners
navigation relies
practitioners on and
and researchers.
the usage
usage ofis2D
researchers. 2D A typical
occupancy
A typical of which
uneven
which
unmapped canterrains
can impede
impede
obstacles orrobot motion.
dynamic
robot
(pits,motion. In stones,
objects,
hillocks, In order togarbage
thereto
order may avoid
avoid these
be small
these
etc.)
Autonomous
approach
problem to navigation of
relies a mobile
on the robot of an important
occupancy unmapped
which obstacles, obstacles
can impede (pits,
robot hillocks,
motion. stones,
In order garbage
to avoid etc.)
these
grid mapsfor
approach
approach tofor
tofor
both
pathpractitioners
navigation
navigation relies
relies on
planning andand
on the
path
the
researchers.
usage
usage of
of 2D
following. A2Dtypical
2D occupancy occu- small
occupancy unmapped obstacleslocal path
(pits, correction
hillocks, is
is typically
stones, applied.
problem
grid maps
approach
grid maps
for
to both
path practitioners
navigation
for path
planning
relies
planning
and
on
and
and
path
the
path
researchers.
usage following.
of 2D
following.
A2D typical
occu-
occupancy
2D occu-
small
which
which
small
One of
obstacles,
can
can impede
impede
obstacles,
the approaches
local
local
robot
robotpathmotion.
path
to
correction
motion.
correction
local path
In order
In correction
order
is togarbage
typically
to
typically
avoid
avoid
is
etc.)
applied.
these
these
applied.
usage of
pancy
approach
pancy
grid grids
grids
maps toforare
are convenient
navigation
convenient
path relies
planning for
foron
andpath
the
path
path planning
usage
planning of 2D
following. and contain
occupancy
and 2Dcontain
occu- small
which
One ofobstacles,
can
the impede
approacheslocalrobotpath
to correction
motion.
local path In is
ordertypically
to
correction avoid
is applied.
these
usage of
grid
pancy maps
grids forare
path planning
convenient forand path
path following.
planning and2D occu- One
contain small
Model ofobstacles,
the approaches
Predictive local
Control path
to correction
local path
(MPC)-based ismethods,
typically
correction is applied.
usage
partiallyof
detailed
grid maps
detailed
pancy information
grids for path
information
are about
planning
about
convenient the
forand
the obstacles
path
obstacles
path around
following.
planning aroundand a
2D
a robot.
occu-
robot.
contain One
small
Model of the approaches
obstacles,
Predictive local
Control to
path local path
correction
(MPC)-based correction
is typically
methods, is usage
applied.
partiallyof
pancy
detailedgrids are
informationconvenient
about for
the path planning
obstacles aroundand acontain
robot. One
Model
Model of the approaches
Predictive
Predictlve Control
Path to local
Integral path
(MPC)-based
(MPPI) correction
methods,
control is usage
partially
(Williams of
However,
detailed
pancy grids
However, when
information
when a robot
are convenient
a robot about operates
the
for
operates path in
obstacles indoor
inplanning
indoor or
aroundand
or industrial
a robot.
contain
industrial One
Model of the approaches
Predictlve
Predictive Path
Control to local
Integral path
(MPPI)
(MPC)-based correction
control
methods, is usage
(Williams
partiallyof
detailed
However, information
when a robotabout the
operates obstacles
in indoor around
or a robot.
industrial Model
et al., Predictive
Predictlve
2016). These Control
Path
methods (MPC)-based
Integral (MPPI)
compute methods,
control
sequence of partially
(Williams
controls
environments,
However,information
detailed
environments, whenit itaoften
often
robot faces
about
faces up obstacles
operates
the
up different
in indoor
different uneven
around
uneven surfaces
or industrial
a robot.
surfaces Model
et al., Predictive
2016).
Predictlve These Control
methods
Path (MPC)-based
Integral compute
(MPPI) methods,
sequence
control of partially
controls
(Williams
However,
environments, whenit aoften
robot operates
faces up interrains,
differentindoor or
uneven industrial
surfaces Model
et al., Predictlve
2016). aaThese Path Integral
methods (MPPI)
compute control of
sequence (Williams
controls
(ramps,
However,
(ramps, stairs,
environments,
environments, whenit
stairs, road
it aoften
road
oftenborders,
robot
borders,
faces
faces
rough
operates
rough
up
up interrains,
different
differentindoor rubbish
rubbish
uneven
uneven on aa for
on
or industrial
surfaces
surfaces
et
Model
for
et
for
robot
al.,
robot
al.,
robot
as
2016).
Predictlve
as
2016).
as a
weighted
These
weighted
These
weighted
methods
Path
methods
average
Integral
average
average
over
compute
(MPPI)
over
compute
over
trajectories
sequence
control of
trajectories
sequence
trajectories of
sampled
controls
(Williams
sampled
controls
sampled
(ramps,
path, stairs,
etc.). These road borders,
surfaces may rough
be terrains, rubbish
traversable for robot on
buta fromfor system
robot as a dynamics.
weighted The
average weights
over for averaging
trajectories sampledare
environments,
path,
(ramps,
(ramps,etc.). These
stairs,
stairs, it often
road
roadsurfaces faces
borders,
borders, mayup bedifferent
rough
rough traversable
terrains,
terrains, uneven
rubbish
rubbish butaa et
surfaces
for robot on
on from
for
from
al., 2016).
system
robot as
system aThese
dynamics.
weighted
dynamics.
methods The
average
The
compute
weights
over
weights
sequence
for
for
of sampled
averaging
trajectories
averaging
controls
are
are
path,
are
path, etc.).
typically
etc.). These
marked
These surfaces
as
surfaces may
obstacles
may be
be traversable
on 2D
traversable for
occupancy
for robot
robot but
grid.
but taken
for
taken
from from
robot
from as
system cost-function
a weighted
cost-function
dynamics. values
average
values
The along
over
along
weights trajectories.
trajectories
trajectories.
for averaging MPPI
sampled
MPPIare
(ramps,
are
path, stairs,
typically
etc.). These road
marked borders,
as
surfaces may rough
obstacles be onterrains,
2D
traversable rubbish
occupancy
for robot on
buta
grid. from
taken system
from dynamics.
cost-function The
values weights
along for averaging
trajectories. MPPIare
are
So, typically
2D marked
navigation as
in such
such obstacles
environments on 2D occupancy
becomes grid.
compli- control
from
control
taken based
system
based
from methods
dynamics.
methods
cost-function are
areThewidely used,
weights
widely
values used,
along but
for
but mainly
averaging
mainly
trajectories. with
are
with
MPPI
path,
So,
are
are 2Detc.).
typically These
navigation
typically marked
marked surfaces
in as
as may
obstacles be traversable
environments
obstacles on
on 2D
2D becomesfor
occupancy
occupancy robot but
compli-
grid.
grid. taken
control from
basedcost-function
methods arevalues
widely along
used, trajectories.
but mainly MPPI
with
So, 2D This
cated. navigation
problem in may
such be environments
solved by becomes detailed
building compli- 2D takenenvironment
control based
from representations.
methods
cost-function are widely
values used,
along but mainlyMPPI
trajectories. with
are
So,
So, typically
cated.
2D
2D This marked
problem
navigation
navigation in as
may
such
in may
such obstacles
be solved
environments
environments on 2D
by occupancy
building
becomes
becomes grid.
detailed
compli- 2D
compli- control
2D environment
based methods
environment representations.
are widely used, but mainly with
representations.
cated.
3D
cated. This
maps,
This problem
but path
problem planning
may be
be solved
in them
solved them by building
by building detailed
takes significant
significant
detailed 2D environment representations.
So, 2D
3D maps,
cated. navigation
This but path
problem in may
such
planningenvironments
be in
solved by becomes
takes
building compli- control
detailed In
2D this based we
work
environment methods
adapt are
MPPI
representations. widely used,
control but mainly
method to with
uneven
3D
3D maps,
computational
maps, but
but path
efforts
path planning
that
planning may in
inbe them
critical
them takes
for
takes significant
low-power
significant In
2D
In this work
environment
this work we
we adapt MPPI
representations.
adapt MPPI control
control method
method to
to uneven
uneven
cated. Thisbut
computational problem
path may
efforts that be may solved by building
be critical detailed terrains
for significant
low-power using 2.5D elevation
MPPImaps. We use local toelevation
3D maps,
computational efforts planning
that mayinbethem critical for low-power In
takes this work we adapt control method uneven
onboard
3D computer.
maps,computer.
onboard
computational
computational but efforts
path planning
efforts that may
that mayinbe bethem critical
critical takes
for low-power terrains
for significant
low-power
In
map this
terrains
terrains
In
map thisas
as
using
work
using
an
using
work
an
2.5D
we
2.5D
input
2.5D
we
input
elevation
adapt
for
adapt
for
MPPImaps.
elevation
MPPI
elevation
MPPI
MPPI
control
maps.
algorithm.
maps.
control
algorithm.
We
We
We
use
use local
method
local
MPPI
use local
method
MPPI
toelevation
is
to
is
uneven
elevation
guided
elevation
uneven
guided
onboard computer. terrains
map as using
an 2.5Dfor
input elevation
MPPI maps.
algorithm.We use local
MPPI elevation
is guided
computational
onboard
Another
onboard computer.
approach
computer. efforts
to that may
navigation be
in critical
uneven for low-power
environments by terrain
terrains
by
map terrain
as traversability
using
an 2.5D
traversability
input elevation
for MPPIvalues
maps.
values computed
We
computed
algorithm. use by
local
by
MPPI this eleva-
elevation
this
is eleva-
Another
onboard
Another
approach to navigation in uneven environments map
computer.
approach to navigation in uneven environments by
tion as anThese
terrain
map. inputtraversability
traversabilityfor MPPI valuesalgorithm.
computed
values are MPPI
by
computed is guided
this guided
eleva-
from
is the usage
usage of height
height maps (2.5Dinmaps).
maps). These maps areare map by as
terrain an input
traversabilityfor MPPIvaluesalgorithm.
computed MPPI
by is
this guided
is the
Another
Another
is the usage
of
approach
approach
of height
to maps
navigation
to maps
navigation(2.5D
(2.5D
uneven
inmaps).
uneven These maps
environments
environments
These maps are by
tion
tion
slope
map.
terrain
map. These
These
steepness,
traversability
traversability
traversability
surface roughness
values
valuesvalues
computed
and
are
are computed
other this eleva-
byparameters.
computed from
eleva-
from
represented
Another
represented
is the usage as
approach
as
of a
a grid
to
grid
height where
navigation
where
maps each
each
(2.5D in cell
uneven
cell
maps). encodes
encodes elevation
environments
These elevation
maps are tion
by
slope map.
terrain These
steepness, traversability
traversability
surface values
roughnessvalues
computed
and are computed
otherby this
parameters.from
eleva-
is the usage
represented of
as height
a grid maps
where (2.5D
each maps).
cell These
encodes maps are
elevation tion
slope
We map.
carry These
steepness,
out wide traversability
surface roughness
experimental values and are
evaluation computed
other parameters.
of our from
nav-
of corresponding
represented
is the
of usage of
corresponding as a area
grid
height
area of
maps
of a
where surface.
each
(2.5D
a surface. Optionally,
cell
maps). encodes
These 2.5D
Optionally, 2.5D
maps grid
elevation tion
We
slope
are slope
grid map.
carry These
out
steepness, wide traversability
surfaceexperimental
roughnessvalues are
evaluation
and computed
other of our
parameters.from
nav-
represented
of corresponding as a grid
area where
of a each
surface. cell encodes
Optionally, elevation
2.5D grid We
igation steepness,
carry out
pipeline surface
widein variousroughness
experimentalsimulated and environments.
other parameters.
evaluation of our nav-
We
maps
of may
represented
maps contain
corresponding as a grid
may contain additional
area of a
where
additional information
surface.
each cell
information like
Optionally,
encodes uncertainty.
2.5D grid
elevation
like uncertainty. slope
igation
We steepness,
carry pipeline
out surface
widein variousroughness
experimentalsimulated and other parameters.
environments.
evaluation of our We
nav-
of
mapscorresponding
may containarea of
additional a surface.
information Optionally, 2.5D
like uncertainty. grid We carry
igation
integrate
igation
outpipeline
pipeline
our
pipeline
wide
in
experimental
in various
various
with ROS2 1 evaluation
simulated
1
simulated framework of
environments. our used
widely
environments.
nav-
We
We
of
mapscorresponding
may
Elevation contain
information area of
additional a surface.
from 2.5D information
2.5D Optionally,
maps is like
islike 2.5D
uncertainty.
widely used grid
to We carry
integrate
igation out
our
pipeline wide
pipeline experimental
with
in various ROS2 1
simulated
1 evaluation
framework of our
widely
environments. nav-
used
We
maps may information
Elevation contain additional from information
maps uncertainty.
widely used to integrate
by robotic
integrate our pipeline
researchers
our pipeline with
and
with ROS2 1
implement
ROS2 1 framework
it
framework as a widely
part
widely ofused
the
used
maps maytraversability
Elevation
estimate contain additional
information from
of rough information
2.5D maps
terrain. is
Thislike uncertainty.
widely used
traversability to igation
by robotic
integrate pipeline
researchers
our in various
pipeline and
with simulated
implement
ROS2 1 environments.
it
framework as aa widely
part of We
the
used
Elevation
estimate information
traversability from
of rough 2.5D maps
terrain. is
This widely used
traversability to by
Nav2 robotic researchers
software stack and
(Macenski implement
et1 al., it as
2020). part of the
Elevation
estimate
may information
traversability
be computed
computed from
of
as binary rough
binary 2.5D maps
terrain.
value is
This
(cellsThis canwidely used
traversability
be classified to
classified integrate
Nav2
by
by robotic
robotic our
software pipeline
stack
researchers
researchers with
(Macenski
and
and ROS2 et
implement
implement framework
al., 2020).
it
it as
as a
a widely
part
part of
ofused
the
the
Elevation
may be
estimate information
traversability as from
of rough 2.5D
value maps
(cells
terrain. iscanwidely
be used to Nav2
traversability software stack (Macenski et al., 2020).
estimate
may
as
as
may
traversability
be computed
traversable
estimate
be computed
of
as binary
or untraversable),
or
rough
untraversable),
traversability
traversable as of rough
binary
terrain.
value
or as
or
value
(cells
as
terrain.
This
cantraversability
a cost-function
a
(cells cost-function
This
can (how by
be classified
traversability
be (how
classified
Nav2
Nav2 robotic researchers
software
software stack
stack
2.
and implement
(Macenski
(Macenski
RELATED
et
et al.,
WORKS
it as a part of the
al., 2020).
2020).
may
as be computed
traversable
difficult to or
traverse as binary
untraversable),
or binary value
how dangerous
dangerousor (cells
as a
the can be
cost-function
region classified
is). (how
These Nav2 software stack
2. (Macenski
RELATED et al.,
WORKS 2020).
as
maytraversable
be to
difficult computed or untraversable),
traverse as
or how or as
value a cost-function
(cells
the can
region be is). (how
classified
These 2. RELATED
2. RELATED WORKS WORKS
as traversable
difficult to
traversability or untraversable),
traverse
values or how
can be dangerousor as
utilized bya cost-function
thepathregion is).
planning (how
These
and 2. RELATED WORKS
difficult
as to
traversable
traversability traverse
or or how
untraversable),
valuesorcan dangerous
bedangerous or as
utilized bythe the
a region
cost-function
path is).
planning These
(how
and 2.1 Elevation Mapping
difficult to traverse
traversability values how
can betoutilized by path region is). These
planning and 2.1 2. RELATED WORKS
path
path following
difficult to traverse
following
traversability algorithms
orcan
algorithms
values how
can compute
bedangerous
toutilized
compute
utilized bytheboth
both
pathregioneffective and
is). These
effective
planning and 2.1 Elevation
Elevation Mapping
Mapping
traversability
pathtrajectory values
following through
algorithms be to compute by path planning
both effective and 2.1 Elevation Mapping
safe
traversability
safe
path followingvalues
pathtrajectory
following through
algorithms
algorithms
uneven
canuneven
beto surface.
toutilized
surface.
compute
compute by path
both planning
both effective and
effective and 2.1 Elevation
Building
2.1 height
Elevation
Building height
Mapping
maps
maps for
Mapping for robot
robot navigation
navigation is is not
not new.
new.
safe
safe trajectory
trajectory through
through uneven
uneven surface.
surface. Building height maps for robot navigation is not new.
new.
path
safe

following
trajectory algorithms
through to
uneven compute
surface. both effective and (Gutmann
Building et
height al., 2005)
maps proposed
for robot building
navigation of height
is not map
 This
safe
 work
trajectory
This work was
was supported
through
supported by Russian
uneven
by Russian Science
surface.
Science Foundation,
Foundation, project
project (Gutmann
Building
(Gutmann et
height al.,
et al., 2005)
al.,maps
2005)for proposed
proposed building
robot navigation
building of of height
of isheight
height map
not new.
map
 This
This work
work was
was supported
supported by
by Russian
Russian Science
Science Foundation,
Foundation, project
project (Gutmann
Building
1 et
height 2005)
maps proposed
for robot building
navigation is not map
new.

22-21-00716
 This work
work was
22-21-00716 was supported
supported by by Russian
Russian Science
Science Foundation,
Foundation, project
project 1 https://docs.ros.org/en/foxy/
(Gutmann et al., 2005) proposed building of height map
https://docs.ros.org/en/foxy/
1 https://docs.ros.org/en/foxy/
 This
22-21-00716 1

This work was supported by Russian Science Foundation, project
22-21-00716
22-21-00716
22-21-00716
(Gutmann
1 et al., 2005) proposed building of height map
1 https://docs.ros.org/en/foxy/
https://docs.ros.org/en/foxy/
https://docs.ros.org/en/foxy/
This work was supported by Russian Science Foundation, project
22-21-00716 1 https://docs.ros.org/en/foxy/

the CC BY-NC-ND license.

22-21-00716 1 https://docs.ros.org/en/foxy/
2405-8963 Copyright © 2022 The Authors. This is an open access article under
Peer review under responsibility of International Federation of Automatic Control.
10.1016/j.ifacol.2023.01.137
 Stepan Dergachev et al. / IFAC PapersOnLine 55-38 (2022) 80–85 81

divided into floor and obstacles for humanoid robot navi- to estimate the traversability of the individual areas of the
gation. In (Mastalli et al., 2017) height grid maps are con- environment, on the basis of which evaluation of trajecto-
structed for safe and efficient motion of four-legged robots. ries can be carried out.More complex methods estimate
(De Gregorio and Di Stefano, 2017) propose construction the slope of the robot in the position, or the degree of
of global 2.5D elevation map as a projection of global 3D surface unevenness nearby of the position: (Tahirovic and
map for Micro Aerial Vehicle (MAV) navigation. Magnani, 2011), (Gu et al., 2008), (Ye and Borenstein,
2004).
A universal, robust and efficient local elevation mapping
method was proposed by (Fankhauser et al., 2018). In their In our work, we use Model Predictive Path Integral
work, local robot-centric elevation map is aggregated from (MPPI) control method for local path following and exam-
range and pose sensor measurements with Kalman filter ine different cost-functions. These cost-functions consider
correction and refined via probabilistic fusion. A source traversability, roughness, and slope angle along the path.
code of this method is commonly available 2 . In our work,
we take this method as a baseline and modify it for efficient
3. PROBLEM STATEMENT
path planning and following.

2.2 Path Planning Consider a differential drive robot tasked to move to the
target location through the uneven terrain. A robot is
One of the most common approaches to robot trajec- equipped with an odometry sensor which provides 6-DoF
tory planning is to represent the environment as a graph robot’s pose and its speed, and an RGB-D sensor which
and find paths in this graph. The vertices of the graph provides point clouds from robot’s front view.
are considered as valid positions or states of the robot, At each moment t, the robot has state st = (pt , qt , vt , wt ),
and an edge between vertices means a transition be- where pt ∈ R3 is the position, qt ∈ R3 is the orientation,
tween corresponding states. The vertices can form a reg- vt ∈ R3 is the transitional velocity, and wt ∈ R3 is the
ular grid (Elfes, 1989; Yap, 2002), or be generated ran- angular velocity. All these are vectors.
domly (Kavraki et al., 1996). One of the most common
algorithms for graph path finding is (Dijkstra et al., 1959), The point cloud Pt = {(xi , yi , zi ); i = 1, . . . , N }, is a set
but currently heuristic search approaches such as A∗ (Hart of N 3D points observed by robot’s RGB-D sensor, where
et al., 1968), T heta∗ (Daniel et al., 2010), D∗ -Lite (Koenig i-th point has coordinates (xi , yi , zi ) in the sensor-related
and Likhachev, 2005) are widely used in this problem. coordinate system.

On the other hand, there is also a group of probabilistic The robot state is changing with the law st+1 = f (st , ut ),
approaches to trajectory planning, such as RRT (Lavalle where ut is a control (input) vector at time step t (e.g. a
et al., 2000) or RRT* (Karaman and Frazzoli, 2011). In vector of linear and angular acceleration which are sent to
this case, the robot states are generated by the algorithm the robot’s controller).
during the planning process by sampling the workspace. The navigation system is also provided with the goal state
For each state its validity is checked on the fly, as well sG = (pg , qg , vg = 0, wg = 0). Its output at step t is a
as the feasibility of the transition from one of the already control input ut which is sent to the robot’s controller in
sampled states. order to reach the goal:
2.3 Path Following in Uneven Terrain
π(st ; P1 , . . . , Pt ) = ut
Safe and efficient robot motion in rough terrains is a
challenging problem, and many works try to address it. Our aim is to build such function π which moves a robot to
(Shimoda et al., 2007) and (Raja et al., 2015) navigate goal pose with spatial and angular deviation under some
in rough terrains using 2.5D grid maps and potential field thresholds:
methods. (Tahirovic and Magnani, 2011) used well-known
RRT algorithm with roughness-based cost-functions which π(sT , P1 , . . . , PT ) = uT ; f (sT , uT ) ≈ sG ,
can also be applied for Model Predictive Control (MPC)
methods. where (pa , qa , va , wa ) ≈ (pb , qb , vb , wb ) means that
MPC methods were used widely for rough terraing locomo- ||pa − pb || < α and ||qa − qb || < β where α and β are the
tion, see (Tahirovic and Magnani, 2010; Fan et al., 2021; pre-defined thresholds.
Buyval et al., 2019). (Fan et al., 2021) solved risk-aware
MPC problem to minimize traversability cost in uneven 4. NAVIGATION SYSTEM
terrains and simultaneously minimizing risk of collision or
tip-over. (Buyval et al., 2019) used MPPI control method 4.1 Overview
for off-road navigation of autonomous truck.
A separate subtask is the evaluation of the trajectories at We propose a fully autonomous navigation system for
uneven terrain. The simplest methods use absolute height uneven environments. The system consists of the following
differences (Shang et al., 2021) or the Euclidean distance modules:
in 3D space between waypoints of the trajectory (Gu and • Elevation mapping module takes RGBD and
Cao, 2011). Other methods, e.g. (Sock et al., 2014), allow odometry sensor data and builds a height map of the
2 https://github.com/ANYbotics/elevation mapping robot’s surroundings.
82 Stepan Dergachev et al. / IFAC PapersOnLine 55-38 (2022) 80–85

• Path planning module builds a global plan to reach |z(i + ∆, j) − z(i − ∆, j)|
the goal. Its inputs are: the robot’s odometry data, Sv (i, j) =
2∆ε
target state, and the height map. Sh + Sv
• Path following module follows the trajectory sug- S(i, j) = ; p(i, j) = e−λ·S(i,j)
gested by the path planner and performs local ma- 2
neuvers to avoid obstacles and severe roughnesses. It where z(i, j) is height value of cell (i, j), ε is the size of the
takes a robot’s odometry data, global path, and 2.5D cell. The adjustment of this method for a specific robot
map built by the mapping module as an input. or environment is made by varying the parameters ∆ and
All the modules run in real time as independent threads. λ. A cell (i, j) is considered traversable if the value p(i, j)
Elevation mapping and path planning modules run at the exceeds 0.5.
low frequency (about 1 Hz). The path follower runs with a
higher frequency (about 10 Hz) to maintain robot’s motion Path Following The MPPI algorithm (Williams et al.,
and quickly react to surface roughness changes. 2016) was chosen to follow the global path. This algorithm
iteratively selects the control to follow the global path
based on the estimation of a set of generated trajectories.
4.2 Mapping
At each step, the algorithm creates a set of control se-
For elevation mapping, we use the approach of (Fankhauser quences. Each control sequence is randomly generated
et al., 2018). It is an open-source 2.5D mapping solution taking into account the kinematic and dynamic constraints
which updates height map from robot’s motion and point of the robot. Based on each sequence of controls and
clouds using Kalman filter and probabilistic vicinity-based the kinematic model of the robot, the robot’s trajectory
map fusion. During map fusion, height and variance of each is built. A cost estimation S(π) is calculated for each
cell are updated from cumulative distribution function trajectory π. The obtained estimates are used to select
(CDF) calculated by points of confidence ellipse around the next control by applying the softmax function to the
the cell. Such probabilistic fusion lets us increase map control sequences using estimated costs.
accuracy and consistency and significantly reduce its un- The cost estimation for individual trajectory π is based on
certainty. Our tests in simulation with noised odometry the following scheme:
show that this method is robust to odometry sensor noise
and is able to build dense and accurate height map.
k

The method of (Fankhauser et al., 2018) has a serious S(π) = αi · Si (π)βi
problem for off-the-shelf navigation with standard RGB- i=0
D sensors. The problem is that RGB-D sensors usually
do not observe space under the robot and directly ahead Parameters αi and βi let us adjust the impact of individual
it. So, at initial stage, the robot stands in an unmapped assessment components Si on the final value.
place, and path follower is unable to move it along 2.5D
The proposed cost-function can be divided into two com-
map. To address this problem, we fill uncovered cells under
ponents: the general part responsible for the quality of
robot’s footprint guided by its spatial position and slope.
the trajectory regardless of elevation and roughness, and
At initial stage, we take larger footprint radius (0.4 meters
the part responsible for evaluating the trajectory from the
in our experiments), and when initialization is complete,
point of view of navigation in an uneven environment.
we fill map cells in smaller radius (0.25 meters), to cover
occasional holes in the map. The general part includes next cost-functions:
• Distance to the trajectory suggested by the path
4.3 Path Planning and Following planner;
• Distance to the goal suggested by the path planner;
Path Planning To create a global path, we suggest to • Backward motion distance - we penalize back-
use any of the planners available in Nav2 ROS2 package. ward motion because the robot has only forward-
For example, Theta* algorithm (Daniel et al., 2010), which looking camera;
is an any-angle modification of the seminal A* algorithm,
can be used. This algorithm operates on a square grids, To evaluate trajectories from the point of view of naviga-
each cell of which should be marked as traversable or as tion in an uneven environment, we suggest two different
untraversable. cost-functions.

Information about the traversability of cells can be ob- The first function is based on the method described
tained on-the-fly based on an elevation map and added in section Path Planning. The value of the function is
on the environment map used in Navigation2 software inversely proportional to the distance to the untraversable
stack. For these purposes, we propose to use the method area closest to the trajectory. Let’s denote this function as
from (Sock et al., 2014). This method builds a probabilistic Slope-Traversability.
estimation p(i, j) of the traversability of a cell (i, j) based The second cost-function is denoted as Slope-Roughness.
on the height difference between cells located at a distance This function is divided into two components which are
of ∆ along the vertical and horizontal axes. sums of inclination and roughness estimates of robot’s
footprint at each trajectory point. The inclination is esti-
|z(i, j + ∆) − z(i, j − ∆)| mated by fitting elevation data at the footprint into a plane
Sh (i, j) = ax + by + c = z using least squares method. The roughness
2∆ε
 Stepan Dergachev et al. / IFAC PapersOnLine 55-38 (2022) 80–85 83

Table 1. The results of experimental eval-

uation of MPPI algorithm with two differ-
ent cost-funtions (Slope-Roughness (SR) and
Slope-Traversability (ST)) at three differ-
ent maps.
Map 1 Map 2 Map 3
Fig. 1. Maps used for experimental evaluation of our ST SR ST SR ST SR
MPPI controller, with start positions (green) and goal Success Rate 100% 100% 100% 100% 98% 98%
positions (red). Path Length 11.36 11.18 11.02 10.99 10.92 11.26
Sim Time 30.51 29.98 28.52 28.05 28.38 31.49
estimate is the standard deviation between points at the
footprint and the estimated plane. • P ath Length – the average length L of the robot’s
path π for successfully completed tasks.
ai , bi , ci = LSM ({(xj , yj , zj ) ∈ Br (πi )}) • Sim T ime – the average simulation time T required
to reach the goal state.
T
 1
Sslope (π) = arccos(  2 (ai , bi , −1) · (0, 0, 1)) Table 1 presents the results of the experimental launch
i=1 a i + b2i + 1 of MPPI algorithm on considered maps with two pro-
posed cost-functions: Slope-Roughness (SR) and Slope-
 Traversability (ST). The results presented in the table
 
 1 (ai xj + bi yj + ci − zj )2 are calculated for tasks successfully completed by both
σi = 
|Br (πi )| a2i + b2i + 1 versions of algorithm. The radius of the robot’s footprint
(xj ,yj ,zj )∈Br (πi )
was set to 0.3 m, the maximum number of simulation steps
T
 nsteps was 1000, and the permissible deviation ∆ of the
Srough (π) = σi robot from the goal was 0.3 m.
i=1
As can be seen in Table 1, both versions of the algorithm
successfully finish the vast majority of tasks. It is worth
5. EXPERIMENTAL EVALUATION
noting that both options build paths avoiding serious
roughness or steep inclines. In the case of the first two
The experimental evaluation of the proposed navigation maps, the algorithm with cost-function Slope-Roughness
system consisted of two parts. In the first part, we per- builds shorter paths on average and reaches the goal
formed an evaluation of the MPPI algorithm. In the second in fewer simulation steps than the algorithm with cost-
part, we carried out the tests of the entire system on a function Slope-Traversability. However, for the third
model on differential-drive robot in the Gazebo simula- map, the opposite results were obtained, the paths of
tor (Koenig and Howard, 2004). MPPI with Slope-Roughness cost-function are longer and
require more simulation steps. This can be explained by
5.1 MPPI Experiments the fact that the Slope-Roughness builds paths that avoid
even small roughness, but allow passage closer to cliffs or
We used three different elevation maps for the experiment. steep inclines.
The first map is a square plane of size 10x10 m with
a truncated cone located in the middle, which has base Examples of trajectories demonstrating this behavior are
diameter of 7 m, top diameter of 3 m, and height of 2.5 m. shown in Fig. 2. It can be seen that the trajectories of
The second map consists of two surfaces having different MPPI with Slope-Traversability (marked in green) are
heights and connected by an even ramp. One surface has located further from the edges of the cone on the first
size 7x8 m, another has size 5x8 m and height 0.5 m above map and from the edge of the cliff on the second map than
first one. The ramp width is 4 m and its slope angle is the trajectories of algorithm MPPI with Slope-Roughness
about 0.1 radians. Third map is a rectangular plane of (marked in red). But at the same time, MPPI with Slope-
size 12 x5 m with two small rectangular pits. The size of Roughness avoids passage through the pit, while MPPI
the pits is 0.5 mx2.5 m, the depth of the pits is 0.05 m. with Slope-Traversability passes through it.
The maps are illustrated in Fig. 1. Such behavior can be obtained due to the possibility of
We generated 100 start and goal positions for each map. more fine-tuning of the cost-function, since two compo-
These positions were distributed randomly as it is shown nents are used, one of which is responsible for assessing
in Fig. 1. In all of the test cases, when moving from start the roughness of the surface, and the other for the slope.
to goal position, the robot had to overcome one of three
obstacles: drive around a truncated cone, drive on a ramp, 5.2 Experiments in Simulation
drive around the pits.
For simulation experiments, we created a world with
The following quality metrics were used to evaluate the
different types of unevenness in Gazebo simulator. The
obtained solutions for N tasks.
world contains a platform of size 3x1 m and height 0.4 m,
• Success Rate – the percentage of tasks for which with two ramps leading to this platform. One ramp is of
the robot has reached the goal position (with an low-grade slope (0.12 rad), and the other ramp is of steep
acceptable specified ∆ deviation) in no more than a slope (0.59 rad). There is a small quadratic pit and small
specified number of simulation steps nsteps . ledge on the platform. The pit has size of 30x30 cm and
84 Stepan Dergachev et al. / IFAC PapersOnLine 55-38 (2022) 80–85

Table 2. Results of experimental evalua-

tion in Gazebo with Slope-Roughness and
Slope-Traversability cost-functions.
Slope-Roughness Slope-Traversability
Path Length, m 26.2 25.9
Travel Time, s 114 119
Fig. 2. Example of trajectories obtained on experimental Step Time, ms 21 90
maps by two variants of the MPPI algorithm (green
trajectories – Slope-Traversability, red trajecto-
ries – Slope-Roughness).

Fig. 3. The environment for simulation experiment.

depth of 2.5 cm. The ledge has size of 3x20 cm and height
of 1.5 cm. Also this world contains small bricks of different
sizes on the ground. The height of the bricks vary from
8 cm to 27 cm, and the width is about 40 cm. Proposed
world is shown in Fig. 3.
We used a model of four-wheeled differential drive robot. Fig. 4. The route for simulation experiments (black dashed
This robot had wheel base of 11 cm, length of 20 cm, width lines) and trajectories traveled by the robot with
of 22 cm, and clearance of 2 cm. It could easily traverse DWA (purple line), MPPI with Slope-Roughness
through the low-slope ramp, and tipped over on the high- (green line) and MPPI with Slope-Traversability
slope ramp. All the bricks were impassable for this robot, (dark blue line). The map color encodes surface ele-
but it was able to traverse through the ledge and the pit vation.
on the platform (with significant swing while entering and
exiting the pit). Maximal linear speed of the robot was 0.5 the number of the simulation steps was 30, the number of
m/s, and maximal rotation speed was 1.3 rad/s. trajectory samples was 300, the footprint radius was set
to 0.2 m. As an occupancy grid for DWB Controller, an
For our tests, we created a route that was divided into elevation map binarized by Slope-Traversability was
ten straight-line segments. First, it entered the platform used. Moreover, unknown area of the elevation map was
using the low-slope ramp. Next, it went to the opposite also considered as untraversable. In order for the robot
end of the platform through the ledge and the pit. Then, not to get close to the obstacles, they were inflated with
it went down to the ground crossing a small part of the an inflating radius of 0.25 m. The resultant trajectories
steep ramp. And finally, it had several loops on the ground, of MPPI and Dynamic Window Approach (DWA) are
crossing the bricks and returning to the start point. A visualized in Fig. 4. The video is available at https://
straight line connecting the robot’s current position and youtu.be/LGhKaxnL8xA.
the next waypoint on a route was used as a reference path
for the path follower. The task of the path follower was to As a result, with both cost-functions, the MPPI success-
go along the route and perform appropriate detours when fully completed the task and avoided collision with the
route segments pass through an obstacle or an undesirable bricks and falling down the steep ramp. The DWA algo-
roughness. rithm overcame the ramp and the platform stages, but
could not build a trajectory to bypass the brick, since this
The point clouds for the elevation mapping module came would require a significant deviation from the reference
from RGBD camera model with resolution 160x120. The path. Also, with both cost-functions, the robot moved
experiments were running on a PC with Intel i9-11900KF around the pit instead of traversing it, but DWA dit not.
CPU (8 cores, 5.1 GHz). Path follower was running on At the same time, all algorithms moved robot through the
a single thread, and other threads were occupied by ledge, because elevation mapping module build flat surface
simulation and elevation mapping. Frequency of controller in place of the ledge.
was set to 10 Hz.
For both cost-functions, traveled time and distance were
The tests were carried out using MPPI algorithm with measured, as well as average time to compute con-
Slope-Roughness and Slope-Traversability cost-functionstrols for one step. The results are shown in Table 2.
and also ROS2 DWB Controller, which is based on well- Slope-Traversability completed the mission a little
known Dynamic Window Approach (Fox et al., 1997). slower but with a slightly less travel distance than
The maximum permissible deviation from the goal was Slope-Roughness, and took much more time for control
set as 0.15 m and 0.25 radian. For the MPPI algorithm computation (90 ms per step compared to 21 ms).
 Stepan Dergachev et al. / IFAC PapersOnLine 55-38 (2022) 80–85 85

6. CONCLUSION AND FUTURE WORK Hart, P.E., Nilsson, N.J., and Raphael, B. (1968). A
formal basis for the heuristic determination of minimum
In this work we proposed a pipeline for navigation of a cost paths. IEEE Transactions on Systems Science and
differential drive robot in uneven terrain, which consists Cybernetics, 4(2), 100–107.
of elevation mapping, path planning, and path following Karaman, S. and Frazzoli, E. (2011). Sampling-based algo-
modules. We tested the proposed pipeline on different syn- rithms for optimal motion planning. The International
thetic maps and evaluated it on a simulated robot. Both Journal of Robotics Research, 30(7), 846–894.
synthetic and simulation experiments showed success rate Kavraki, L.E., Svestka, P., Latombe, J.C., and Overmars,
close to 100% and proved that proposed path following M.H. (1996). Probabilistic roadmaps for path planning
method is able to safely navigate through uneven terrain in high-dimensional configuration spaces. IEEE trans-
avoiding obstacles and large roughnesses. actions on Robotics and Automation, 12(4), 566–580.
In the future, we plan to create a more efficient imple- Koenig, N. and Howard, A. (2004). Design and use
mentation of the MPPI algorithm by parallelizing com- paradigms for gazebo, an open-source multi-robot sim-
putations using CUDA/OpenCL toolkits. Another area ulator. In IROS 2004, 2149–2154. Sendai, Japan.
of future work is increasing the robustness of MPPI and Koenig, S. and Likhachev, M. (2005). Fast replanning for
adapting this approach to a larger class of dynamic sys- navigation in unknown terrain. IEEE Transactions on
tems by using the ideas described in (Gandhi et al., 2021) Robotics, 21(3), 354–363.
and (Williams et al., 2017). Finally, conducting experi- Lavalle, S.M., Kuffner, J.J., and Jr. (2000). Rapidly-
ments on a real robot is also a perspective direction of exploring random trees: Progress and prospects. In Al-
future work. gorithmic and Computational Robotics: New Directions,
293–308.
REFERENCES Macenski, S., Martin, F., White, R., and Ginés Clavero, J.
(2020). The marathon 2: A navigation system. In IROS
Buyval, A., Gabdullin, A., Gafurov, S., Fedorenko, R., 2020.
and Lyubimov, M. (2019). The architecture of the self- Mastalli, C., Focchi, M., Havoutis, I., Radulescu, A.,
driving car project at innopolis university. In DeSE Calinon, S., Buchli, J., Caldwell, D.G., and Semini, C.
2019, 504–509. (2017). Trajectory and foothold optimization using low-
Daniel, K., Nash, A., Koenig, S., and Felner, A. (2010). dimensional models for rough terrain locomotion. In
Theta*: Any-angle path planning on grids. Journal of ICRA 2017, 1096–1103.
Artificial Intelligence Research, 39, 533–579. Raja, R., Dutta, A., and Venkatesh, K.S. (2015). New
De Gregorio, D. and Di Stefano, L. (2017). Skimap: An potential field method for rough terrain path planning
efficient mapping framework for robot navigation. In using genetic algorithm for a 6-wheel rover. Robotics
ICRA 2017, 2569–2576. and Autonomous Systems, 72, 295–306.
Dijkstra, E.W. et al. (1959). A note on two problems in Shang, G., Liu, G., Zhu, P., Han, J., et al. (2021). Com-
connexion with graphs. Numerische mathematik, 1(1), plete coverage path planning for horticultural electric
269–271. tractors based on an improved genetic algorithm. Jour-
Elfes, A. (1989). Using occupancy grids for mobile robot nal of Applied Science and Engineering, 24(3), 447–456.
perception and navigation. Computer, 22(6), 46–57. Shimoda, S., Kuroda, Y., and Iagnemma, K. (2007).
Fan, D.D., Otsu, K., Kubo, Y., Dixit, A., Burdick, J., High-speed navigation of unmanned ground vehicles on
and Agha-Mohammadi, A.A. (2021). Step: Stochastic uneven terrain using potential fields. Robotica, 25(4),
traversability evaluation and planning for risk-aware off- 409–424.
road navigation. In Robotics: Science and Systems, 1–21. Sock, J., Kwak, K., Min, J., and Park, Y.W. (2014). Proba-
Fankhauser, P., Bloesch, M., and Hutter, M. (2018). Prob- bilistic traversability map building for autonomous nav-
abilistic terrain mapping for mobile robots with un- igation. In ICCAS 2014, 652–655.
certain localization. IEEE Robotics and Automation Tahirovic, A. and Magnani, G. (2010). Passivity-based
Letters, 3(4), 3019–3026. model predictive control for mobile robot navigation
Fox, D., Burgard, W., and Thrun, S. (1997). The dynamic planning in rough terrains. In IROS 2010, 307–312.
window approach to collision avoidance. IEEE Robotics Tahirovic, A. and Magnani, G. (2011). A roughness-
and Automation Magazine, 4(1), 23–33. based rrt for mobile robot navigation planning. IFAC
Gandhi, M.S., Vlahov, B., Gibson, J., Williams, G., and Proceedings Volumes, 44(1), 5944–5949.
Theodorou, E.A. (2021). Robust model predictive path Williams, G., Drews, P., Goldfain, B., Rehg, J.M., and
integral control: Analysis and performance guarantees. Theodorou, E.A. (2016). Aggressive driving with model
IEEE Robotics and Automation Letters, 6(2), 1423– predictive path integral control. In ICRA 2016, 1433–
1430. 1440.
Gu, J. and Cao, Q. (2011). Path planning for mobile robot Williams, G., Wagener, N., Goldfain, B., Drews, P., Rehg,
in a 2.5-dimensional grid-based map. Industrial Robot: J.M., Boots, B., and Theodorou, E.A. (2017). Infor-
An International Journal. mation theoretic mpc for model-based reinforcement
Gu, J., Cao, Q., and Huang, Y. (2008). Rapid traversabil- learning. In ICRA 2017, 1714–1721.
ity assessment in 2.5 d grid-based map on rough ter- Yap, P. (2002). Grid-based path-finding. In AI 2002, 44–
rain. International Journal of Advanced Robotic Sys- 55.
tems, 5(4), 40. Ye, C. and Borenstein, J. (2004). A method for mobile
Gutmann, J.S., Fukuchi, M., and Fujita, M. (2005). A robot navigation on rough terrain. In ICRA 2004,
floor and obstacle height map for 3d navigation of a volume 4, 3863–3869.
humanoid robot. In IRCA 2005, 1066–1071.