# STEP: Stochastic Traversability Evaluation and Planning for Risk-Aware Off-
road Navigation; Results from the DARPA Subterranean Challenge

Anushri Dixit∗1, David D. Fan∗2, Kyohei Otsu2, Sharmita Dey2, Ali-Akbar Agha-
Mohammadi2, and Joel Burdick1

1 California Institute of Technology
2 NASA Jet Propulsion Laboratory, California Institute of Technology
* Indicates equal contribution.
©2023, California Institute of Technology. All Rights Reserved

###### Abstract

Although autonomy has gained widespread usage in structured and controlled
environments, robotic autonomy in unknown and off-road terrain remains a
difficult problem. Extreme, off-road, and unstructured environments such as
undeveloped wilderness, caves, rubble, and other post-disaster sites pose
unique and challenging problems for autonomous navigation. Based on our
participation in the DARPA Subterranean Challenge, we propose an approach to
improve autonomous traversal of robots in subterranean environments that are
perceptually degraded and completely unknown through a traversability and
planning framework called STEP (Stochastic Traversability Evaluation and
Planning). We present 1) rapid uncertainty-aware mapping and traversability
evaluation, 2) tail risk assessment using the Conditional Value-at-Risk
(CVaR), 3) efficient risk and constraint-aware kinodynamic motion planning
using sequential quadratic programming-based (SQP) model predictive control
(MPC), 4) fast recovery behaviors to account for unexpected scenarios that may
cause failure, and 5) risk-based gait adaptation for quadrupedal robots. We
illustrate and validate extensive results from our experiments on wheeled and
legged robotic platforms in field studies at the Valentine Cave, CA (cave
environment), Kentucky Underground, KY (mine environment), and Louisville Mega
Cavern, KY (final competition site for the DARPA Subterranean Challenge with
tunnel, urban, and cave environments).

##  1 Introduction

Figure 1: Top left: Boston Dynamics Spot quadruped robot exploring Valentine
Cave at Lava Beds National Monument, CA. Top middle, bottom middle (second
image from the left): Clearpath Husky robot exploring Arch Mine in Beckley,
WV. Bottom left: Spot exploring abandoned Satsop power plant in Elma, WA.

Robust autonomous traversal over extreme, hazardous terrain is an open problem
with many applications to extra-terrestrial [42], disaster-struck [1], and
subterranean environments [34]. The robots operate in uneven and highly risky
terrain based on the use of noisy sensor measurements and localization
uncertainty. Autonomous motion planning in such conditions requires a
framework that can account for the traversability risk arising from sources
like rubble, sudden drops, muddy/slippery areas, while also considering the
uncertainty in the robots estimates. The framework must be able to make
tractable reformulations of the nonconvex constraints arising from these risks
so that it can plan reactive motions in real time.

In this work, our evaluation of a terrain’s traversability accounts for the
hazards from the uneven terrain (geometric, confidence-aware analysis) and
type of terrain (semantic analysis), see Figure 1. We will treat this
traversability cost as a random variable and account for the uncertainty in
the traversability cost using Conditional Value-at-Risk (CVaR). Using the CVaR
assessment of the terrain, we obtain a geometric path and a low-level model
predictive control (MPC) plan that accounts for the risk in the terrain.

Most traversability analyses are dependent on the types of sensors used to
obtain information of the terrain, and are based on geometry, appearance, or
proprioceptive methods [45]. Geometry-based methods often rely on building a
2.5D terrain map which is used to extract features such as maximum, minimum,
and variance of the terrain height and slope [28]. Planning algorithms for
such methods take into account the stability of the robot on the terrain [29].
In [43, 27], the authors estimate the probability distributions of states
based on the kinematic model of the vehicle and the terrain height
uncertainty. Furthermore, a method for incorporating sensor and state
uncertainty to obtain a probabilistic terrain estimate in the form of a grid-
based elevation map was considered in [23]. Learning-based traversability
frameworks are becoming increasingly popular. In [26], the authors proposed an
inverse reinforcement learning-based method for learning the traversability
cost from raw exterioceptive and proprioceptive sensor measurements. In [10],
the authors learn a speed-map representation given semantic information while
in [39], the authors use a self-supervised learning-based terrain roughness
classifier. In [22], the authors proposed a neural network architecture for
learning the distribution of traversability costs arising from geometric risks
using the pointcloud obtained from a LiDAR in a supervised manner. In this
paper, we build upon the traversability analyses of classical geometric
methods, while using semantic information from a LiDAR and incorporating the
uncertainty of these methods for risk-aware planning [2, 51, 20].

Risk can be incorporated into motion planning using a variety of different
methods, including chance constraints [42, 52], exponential utility functions
[35], distributional robustness [54, 53], and quantile regression [21, 13].
Risk measures, often used in finance and operations research, provide a
mapping from a random variable (usually the cost) to a real number. These risk
metrics should satisfy certain axioms in order to be well-defined as well as
to enable practical use in robotic applications [38]. Conditional value-at-
risk (CVaR) is one such risk measure that has this desirable set of
properties, and is a part of a class of risk metrics known as coherent risk
measures [7]. Coherent risk measures have been used in a variety of decision
making problems, especially Markov decision processes (MDPs) [12, 4, 3, 5].
Coherent risk measures have been used in a MPC framework when the system model
is uncertain [49, 15], when the uncertainty is a result of measurement noise
or moving obstacles [30, 18], and when the uncertainty arises from both
measurement and process noise [17]. Our work extends CVaR risk to a risk-based
planning framework which utilizes different sources of traversability risk
(such as collision risk, step risk, slippage risk, etc.) Morever, this paper
introduces the first field-hardened and theoretically grounded approach to
traversability assessment and risk-constrained planning using CVaR metrics.
Using CVaR to assess traversability risks allows us to dynamically tune the
entire system’s behavior - from aggressive to highly conservative - by
changing a single value, the risk level. When the value of the risk level is
small, the behavior of the traversability assessment is equivalent to an
expectation-based assessment, which is similar to the geometric traversability
evaluation in the past literature. Conversely, when the risk level is high, we
account for the worst possible value of our terrain assessment given the
uncertainty in our measurements.

Model Predictive Control has long been used to robustly control more complex
systems, including time-varying, nonlinear, or MIMO systems [11]. While simple
linear PID controllers are sufficient for simpler systems, MPC is well-suited
to more complex tasks while being computationally feasible - MPC can account
for the nonlinear robot dynamics while making multi-step predictions of the
trajectory followed by the robot, and incorporating complex state and control
constraints such as CVaR constraints, obstacle avoidance constraints, and non-
holonomic constraints. Hence, MPC provides an optimization-based framework to
mimic a suboptimal infinite-horizon controller while being computationally
tractable. In particular, we employ Sequential Quadratic Programming (SQP),
which iteratively solves locally quadratic sub-problems to converge to a
globally (more) optimal solution [9]. This approach reduces computational
costs and flexibility for handling a wide variety of costs and constraints
[47, 36, 40, 8, 25]. A common criticism of SQP-based MPC methods (and
nonlinear MPC methods in general) is that they are susceptible to local
minima. We address this problem by incorporating a trajectory library (which
can be predefined and/or randomly generated, e.g. as in [33]) to use in a
preliminary trajectory selection process. This approach finds more globally
optimal initial guesses for the SQP problem to refine locally. Another common
difficulty with risk-constrained nonlinear MPC problems is ensuring recursive
feasibility [37]. We bystep this problem by dynamically relaxing the severity
of the risk constraints while penalizing CVaR in the cost function.

In this work, we propose STEP (Stochastic Traversability Evaluation and
Planning), that pushes the boundaries of the state-of-the-practice to enable
safe, risk-aware, and high-speed ground traversal of unknown environments.
Specifically, our contributions include:

  1. 1.

Uncertainty-aware 2.5D traversability evaluation which accounts for
localization error, sensor noise, and occlusion, and combines multiple sources
of traversability risk.

  2. 2.

An approach for combining these traversability risks into a unified risk-aware
CVaR planning framework.

  3. 3.

A highly efficient MPC architecture for robustly solving non-convex risk-
constrained optimal control problems.

  4. 4.

A suite of recovery behaviors to account for fast response to failure
scenarios.

  5. 5.

Risk-based gait adaptation for quadrupedal robots (in our case, the Boston
Dynamics Spot platform).

  6. 6.

Real-world demonstration of real-time CVaR planning on wheeled and legged
robotic platforms in unknown and risky environments.

This work significantly extends our previous work [20] to incorporate semantic
and confidence-aware traversability risk sources in addition to geometric ones
to provide a complete suite of traversability analyses using LiDARs. Further,
we provide recovery behaviors in our hierarchical planning framework to
account for high-risk situations that the geometric and kinodynamic planners
fail to account for. Lastly, we also provide risk-aware gait selection
techniques for quadrupedal robots. This hierarchical framework is tested
extensively in multiple subterranean environments.

##  2 Risk-Aware Traversability and Planning

###  2.1 Problem Statement

Let $x_{k}$, $u_{k}$, $z_{k}$ denote the robot’s state, action (or control
input), and observation (or sensory measurement) at the $k$-th time step. A
path $x_{0:N}=\\{x_{0},x_{1},\cdots,x_{N}\\}$ is composed of a sequence of
poses. A policy is a mapping from state to control $u_{k}=\pi(x_{k})$. A map
is represented as $m=(m^{(1)},m^{(2)},\cdots)$ where $m^{i}$ is the $i$-th
element of the map (e.g., a cell in a grid map or feature in a feature-based
map). The robot’s dynamics model captures the physical properties of the
vehicle’s motion, such as inertia, mass, dimension, shape, and kinematic and
control constraints:

$\displaystyle x_{k+1}$ $\displaystyle=f(x_{k},u_{k})$ (1) $\displaystyle
g(u_{k})$ $\displaystyle\succ 0$ (2) $\displaystyle h(m,x_{k})$
$\displaystyle\succ 0$ (3)

where $g(u_{k}),$ $h(m,x_{k})$ are vector-valued functions that encode the
control and state constraints/limits respectively.

Following [45], we define traversability as the capability for a ground
vehicle to reside over a terrain region under an admissible state that
satisfies constraints. We represent traversability as a cost, i.e. a
continuous value computed using a terrain model, the robotic vehicle model,
and kinematic constraints, which represents the degree to which we wish the
robot to avoid a given state:

$\displaystyle r=\mathcal{R}(m,x,u)$ (4)

where $r\in\mathbb{R}$, and $\mathcal{R}(\cdot)$ is a terrain assessment
model. This model captures various unfavorable events such as collision,
getting stuck, tipping over, high slippage, to name a few. Each mobility
platform has its own assessment model to reflect its mobility capability.

Associated with the true traversability value is a distribution over possible
values based on the current understanding about the environment and robot
actions. In most real-world applications where perception capabilities are
limited and noisy, the true value can be highly uncertain. To handle this
uncertainty, consider a map belief, i.e., a probability distribution
$p(m|x_{0:k},z_{0:k})$, over a possible set $\mathcal{M}$. Then, the
traversability estimate is also represented as a random variable
$R:(\mathcal{M}\times\mathcal{X}\times\mathcal{U})\longrightarrow\mathbb{R}$.
We call this probabilistic mapping from map belief, state, and controls to
possible traversability cost values a terrain assessment model.

A risk metric $\rho(R):R\rightarrow\mathbb{R}$ is a mapping from a random
variable to a real number which quantifies some notion of risk. In order to
assess the risk of traversing along a path $x_{0:N}$ with a policy $\pi$, we
wish to define the cumulative risk metric associated with the path,
$J(x_{0},\pi)$. To do this, we need to evaluate a sequence of random variables
$R_{0:N}$. To quantify the stochastic outcome as a real number, we use the
dynamic, time-consistent risk metric given by compounding the one-step risk
metrics [46]:

$\displaystyle J(x_{0},\pi;m)$
$\displaystyle=R_{0}+\rho_{0}\big{(}R_{1}+\rho_{1}\big{(}R_{2}+\dotsc+\rho_{N-1}\big{(}R_{N})\big{)}\big{)}$
(5)

where $\rho_{k}(\cdot)$ is a one-step coherent risk measure at time $k$. This
one-step risk gives us the cost incurred at time-step $k+1$ from the
perspective of time-step $k$. Any distortion risk measure compounded as given
in (5) is time-consistent (see [38] for more information on distortion risk
metrics and time-consistency). We use the Conditional Value-at-Risk (CVaR) as
the one-step risk metric:

$\displaystyle\rho(R)=\mathrm{CVaR}_{\alpha}(R)=$
$\displaystyle\inf_{z\in\mathbb{R}}\mathbb{E}\Bigg{[}z+\frac{(R-z)_{+}}{1-\alpha}\Bigg{]}$
(6)

where $(\cdot)_{+}=\max(\cdot,0)$, and $\alpha\in(0,1]$ denotes the risk
probability level. We note that the results in this paper are also easily
extended to other tail risk measures like Entropic Value-at-Risk [6] and total
variation distance-based risk [16].

We formulate the objective of the problem as follows: Given the initial robot
configuration $x_{S}$ and the goal configuration $x_{G}$, find an optimal
control policy $\pi^{*}$ that moves the robot from $x_{S}$ to $x_{G}$ while 1)
minimizing time to traverse, 2) minimizing the cumulative risk metric along
the path, and 3) satisfying all kinematic and dynamic constraints. For a
quadrupedal robot like the Boston Dynamics Spot robot, the framework must
additionally also select the best gait type based on the risk accrued while
moving from $x_{S}$ to $x_{G}$ as a part of the optimal policy $\pi^{*}$.

###  2.2 Hierarchical Risk-Aware Planning

We propose a hierarchical approach to address the aforementioned risk-aware
motion planning problem by splitting the motion planning problem into
geometric and kinodynamic domains. We consider the geometric domain over long
horizons, while we solve the kinodynamic problem over a shorter horizon. This
is convenient for several reasons: 1) Solving the full constrained CVaR
minimization problem over long timescales/horizons becomes intractable in
real-time. 2) Geometric constraints play a much larger role over long
horizons, while kinodynamic constraints play a much larger role over short
horizons (to ensure dynamic feasibility at each timestep). 3) A good estimate
(upper bound) of risk can be obtained by considering position information
only. This is done by constructing a position-based traversability model
$\mathcal{R}_{\mathrm{pos}}$ by marginalizing out non-position related
variables from the terrain assessment model, i.e. if the state
$x=[p_{x},p_{y},x_{\mathrm{other}}]^{\intercal}$ consists of position and non-
position variables (e.g. orientation, velocity), then

$\displaystyle\mathcal{R}_{\mathrm{pos}}(m,p_{x},p_{y})\geq\mathcal{R}(m,x,u)\quad\forall
x_{\mathrm{other}},u$ (7)

Geometric Planning: The objective of geometric planning is to search for an
optimistic risk-minimizing path, i.e. a path that minimizes an upper bound
approximation of the true CVaR value. For efficiency, we limit the search
space only to the geometric domain. We search for a sequence of poses
$x_{0:N}$ which ends at $x_{G}$ and minimizes the position-only risk metric in
(5), which we define as $J_{\mathrm{pos}}(x_{0:N})$. The path optimization
problem can be written as:

$\displaystyle
x_{0:N}^{*}=\operatorname*{arg\,min}_{x_{0:N}}\bigg{[}J_{\mathrm{pos}}(x_{0:N})$
$\displaystyle+\lambda\sum_{k=0}^{N-1}\|x_{k}-x_{k+1}\|^{2}\bigg{]}$ (8)
$\displaystyle s.t.\quad\phi(m,x_{k})$ $\displaystyle\succ 0$ (9)

where the constraints $\phi(\cdot)$ encode position-dependent traversability
constraints (e.g. constraining the vehicle to avoid obstacles and prohibit
lethal levels of risk) and $\lambda\in\mathbb{R}$ weighs the tradeoff between
risk and path length.

Figure 2: Overview of system architecture for STEP. From left to right:
Odometry aggregates sensor inputs and relative poses. Next, Risk Map
Processing merges these pointclouds and creates a multi-layer risk map. The
map is used by the Geometric Path Planner and the Kinodynamic MPC Planner. An
optimal trajectory is found and sent to the Tracking Controller, which
produces control inputs to the robot.

Kinodynamic Planning: We then solve a kinodynamic planning problem to track
the optimal geometric path, minimize the risk metric, and respect kinematic
and dynamics constraints. The goal is to find a control policy $\pi^{*}$
within a local planning horizon $T\leq N$ which tracks the path $X^{*}_{0:N}$.
The optimal policy can be obtained by solving the following optimization
problem:

$\displaystyle\pi^{*}=\operatorname*{arg\,min}_{\pi\in\Pi}\bigg{[}J(x_{0},\pi)+$
$\displaystyle\lambda\sum_{k=0}^{T}\|x_{k}-x^{*}_{k}\|^{2}\bigg{]}$ (10)
$\displaystyle s.t.~{}\forall k\in[0,\cdots,T]:\qquad x_{k+1}$
$\displaystyle=f(x_{k},u_{k})$ (11) $\displaystyle g(u_{k})$
$\displaystyle\succ 0$ (12) $\displaystyle h(m,x_{k})$ $\displaystyle\succ 0$
(13)

where the constraints $g(u)$ and $h(m,x_{k})$ are vector-valued functions
which encode controller limits and state constraints, respectively.

Recovery from Unfavorable States: Recovery planning is a particular type of
planning problem where the initial state is not safe. For example, the robot
might need to start planning from the state where it touches walls with its
bumpers, or the state where the body is tilted on top of rubble. The recovery
from those unfavorable states involves finding a safe control without
violating the safety constraints under smaller margin conditions.

##  3 STEP for Unstructured Terrain

This section discusses how we compute traversability risk and efficiently
solve the risk-aware trajectory optimization problem. At a high level, our
approach takes the following steps (see Figure 2): 1) Aggregate sensor
measurements to create an uncertainty-aware map using some source of
localization with uncertainty, 2) Perform ground segmentation to isolate the
parts of the map that the robot can potentially traverse. 3) Compute risk and
risk uncertainty using geometric properties of the pointcloud (optionally,
include other sources of risk, e.g. semantic or other sensors). 4) Aggregate
these risks to compute a 2.5D CVaR risk map. 5) Solve for an optimistic CVaR
minimizing path over long ranges with a geometric path planner. 7) Solve for a
kinodynamically feasible trajectory which minimizes CVaR while staying close
to the geometric path and satisfying all constraints.

###  3.1 Pointcloud Processing and Mapping

Multi-sensor Merging: Our pointcloud pipeline starts from merging pointclouds
from different sensors. One robot can have multiple units of the same senor to
increase coverage, or have heterogeneous sensors that produce pointclouds
using different mechanics (e.g., active LiDAR, RGBD cameras, passive stereo
cameras). After applying sensor-specific filters that remove noise or body
occlusion, these pointclouds are merged using extrinsic calibration
information. If the sensors are not time-synchronized, we use odometry to
compensate motion offset.

Temporal Pointcloud Merging: The merged pointclouds are aggregated over a
fixed time window to construct a local pointcloud map. We maintain a pose
graph on the time window to incorporate history updates in the odometry
estimate (e.g., loop closures). Based on the latest pose graph, we reconstruct
a full aggregated pointcloud. We annotate each point by the time offset to the
latest pointcloud. This allows us to propagate odometry uncertainty to each
point in the fusion phase.

Ground Segmentation: The aggregated pointcloud is segmented into ground and
obstacle points using 3D pointcloud segmentation techniques. We leverage the
work in [31] that allows efficient ground segmentation based on line fitting
in the cylindrical coordinates. We extended the work to also handle challenges
prevalent in subterranean environments, such as low ceiling or negative
obstacles. The ground segmentation is critical for elevation mapping in an
occluded environment where the ground is not observed by the sensors and the
measurements of walls/ceiling make false ground planes.

Elevation Mapping: We construct 2.5D height map using the segmented ground
points. After splitting points to each grid map cell and sorting by time
stamps, we apply a Kalman filter to estimate the height of the ground. We set
up the filter to place more weight on the recent measurements which are less
affected by the odometry error. Figure 3 shows the visualization of ground
height estimation with uncertainty. Note that there is higher level of
uncertainty for the areas that have few measurements. These mapping
uncertainties are used to adjust confidence in a later traversability
estimation process.

Figure 3: Estimation of ground height with uncertainty

###  3.2 Modeling Assumptions

Among many representation options for rough terrain, we use a 2.5D grid map in
this paper for its efficiency in processing and data storage [24]. The map is
represented as a collection of terrain properties (e.g., height, risk) over a
uniform grid.

For different vehicles we use different robot dynamics models. For example,
for a system which produces longitudinal/lateral velocity and steering (e.g.
legged platforms), the state and controls can be specified as:

$\displaystyle x$
$\displaystyle=[p_{x},p_{y},p_{\theta},v_{x},v_{y},v_{\theta}]^{\intercal}$
(14) $\displaystyle u$ $\displaystyle=[a_{x},a_{y},a_{\theta}]^{\intercal}$
(15)

While the dynamics $x_{k+1}=f(x_{k},u_{k})$ can be written as,

$\displaystyle x_{k+1}=x_{k}+\Delta
t\begin{bmatrix}v_{x}\cos(p_{\theta})-v_{y}\sin(p_{\theta})\\\
v_{x}\sin(p_{\theta})+v_{y}\cos(p_{\theta})\\\ \kappa
v_{x}+(1-\kappa)v_{\theta}\\\ a_{x}\\\ a_{y}\\\ a_{\theta}\end{bmatrix}.$

We let $\kappa\in[0,1]$ be a constant which adjusts the amount of turning-in-
place the vehicle is permitted. In differential drive or ackermann steered
vehicles we can remove the lateral velocity component of these dynamics.
However, our general approach is applicable to any vehicle dynamics model.
(For a differential drive model, see Appendix A)

###  3.3 Terrain assessment models

The traversability cost is assessed as the combination of multiple risk
factors. These factors are designed to capture potential hazards for the
target robot in the specific environment (Figure 4). Such factors include:

  * •

Collision: quantified by the distance to the closest obstacle point.

  * •

Step size: the height gap between adjacent cells in the grid map.

  * •

Tip-over: a function of slope angles and the robot’s orientation.

  * •

Sensor Uncertainty: sensor and localization error increase the variance of
traversability estimates.

  * •

Negative Obstacles: detected by checking the lack of measurement points in a
cell.

  * •

Slippage: quantified by geometry and the surface material of the ground.

####  3.3.1 Geometry-based risk sources

Geometry-based risk sources include collision with obstacles, too-large step
sizes in terrain, and impassable slopes. These geometry-based risks are
constructed using geometric analysis of elevation map estimates and LiDAR
pointcloud points. We construct these risks per grid cell, with the following
methods. First, using the ground estimates from the ground segmentation
pipeline, we obtain a lower bound on the height of the ground, i.e. the height
at which the robot would place its foot or wheel if occupying that cell with
its foot/wheel. Above this ground estimate, we can determine the relative
height of other occupying LiDAR points in the temporally merged pointcloud
with respect to the ground. Points which occur above or below a certain height
threshold which outline the body of the robot are treated as obstacles, and
the corresponding grid cells at these points are marked as untraversable.
Similar analysis is performed for step size risk, which checks the height gap
between adjacent cells in the elevation map. For adjacent cells which exhibit
too high a height gap, these cells are marked as untraversable. Finally,
checking the normals of the elevation map [24] gives an overall estimation of
slope (which the size of the normal calculation averaged over the approximate
size of the robot), and areas with too high slope values are marked as high
risk or untraversable.

Note that in all these analyses, the uncertainty of the elevation map plays a
large role. Sensor and localization uncertainty corrupts elevation
measurements to a varying degree, proportional with distance from the robot.
Therefore we adapt various detection or risk thresholds with distance from the
robot, to obtain a more robust result. For example, for the Husky robot,
ground clearance beneath the robot belly is 10cm. Detecting a 10cm step size
in elevation at 100m away requires an angular localization accuracy of < 0.06
degrees. This may be infeasbile, and therefore adapting the step size
threshold with distance helps to reduce spurious detections at longer ranges.

Figure 4: Multi-layer geometric risk analysis, which first aggregates recent
pointclouds (top). Then, each type of analysis (slope, step, collision, etc.)
generates a risk map along with uncertainties (middle rows). These risks are
aggregated to compute the final CVaR map (bottom).

####  3.3.2 Confidence-based risk sources

One example of confidence-based risk sources is the detection of negative
obstacles. To detect negative obstacles, we estimate areas in the pointcloud
that have no returns and use 2-D ray-tracing to find "gap" areas that are not
occluded by obstacles, small steps, or upward slopes. However, if this is the
only criteria for negative obstacle detection, many false positives for
negative obstacles are observed. One such instance is shown in Fig 5 when the
robot turns the corner into a new room from a narrow passageway and in the
time-taken for the LiDAR returns from all areas of the new room to reach the
sensor, false-positives of negative obstacles are detected. To address this,
we account for the confidence in the gaps in the pointcloud by estimating
whether these areas have been sufficiently covered so far, i.e., the robot has
sensed the area from different positions and for long enough to ensure that
the gaps in the pointcloud are not caused due to the sensor and environment
configuration, see Algorithm 1 for a detailed description of the method to
check whether a given region passes the coverage check.

Algorithm 1 Algorithm for computing whether a given area is unoccluded and
sufficiently covered, i.e., $m^{\text{cover}}_{\text{MSE}}>=0$

0: $m^{\text{unocc}}$: Grid map of unoccluded grid points from the current
robot state $x_{t}$ (using ray-tracing),
$x_{t}=[p_{x},p_{y},p_{\theta},v_{x},v_{y},v_{\theta}]^{T}$: Current robot
state, $d^{\text{cover}}$: Threshold for sufficient coverage (in meters).

0: $m_{N}^{\text{unocc}}$: Number of times each grid cell in the grid map has
been observed unoccluded by the robot so far,
$m^{\text{cover}}=\\{m^{\text{cover}}_{\mu_{x}},m^{\text{cover}}_{\mu_{y}},m^{\text{cover}}_{\text{MSE}}\\}$:
Multi-layer map indicating whether each grid cell being evaluated for presence
of negative obstacles has been observed unoccluded from different points to
ensure sufficient coverage.

1: for $i=0$ to $|m^{\text{unocc}}|$ do

2: # Update $m_{N}^{\text{unocc}}$
$m_{N}^{\text{unocc},(i)}=\left\\{\begin{array}[]{ll}1+m_{N}^{\text{unocc},(i)}&\quad\text{if
}m^{\text{unocc},(i)}=1\text{ (i.e., $i^{\text{th}}$ grid cell is
unoccluded)}\\\ m_{N}^{\text{unocc},(i)}&\quad\text{if
}m^{\text{unocc},(i)}=0\text{ (i.e., $i^{\text{th}}$ grid cell is
occluded)}\end{array}\right.$

3: # Update mean $x$ and $y$ robot positions from where each grid cell has
been observed unoccluded
$\\{m^{{\text{cover}},(i)}_{\mu_{x}},m^{{\text{cover}},(i)}_{\mu_{y}}\\}=\left\\{\begin{array}[]{ll}\\{m^{{\text{cover}},(i)}_{\mu_{x}}+\frac{p_{x}-m^{{\text{cover}},(i)}_{\mu_{x}}}{m_{N}^{\text{unocc},(i)}},m^{{\text{cover}},(i)}_{\mu_{y}}+\frac{p_{y}-m^{{\text{cover}},(i)}_{\mu_{y}}}{m_{N}^{\text{unocc},(i)}}\\}&\quad\text{if
}m^{\text{unocc},(i)}=1\\\
\\{m^{{\text{cover}},(i)}_{\mu_{x}},m^{{\text{cover}},(i)}_{\mu_{y}}\\}&\quad\text{if
}m^{\text{unocc},(i)}=0\end{array}\right.$

4: # Update mean squared deviation of the average robot positions from which a
given grid cell is observed to check if it crosses the threshold for
sufficient coverage, $d^{\text{cover}}$
$m^{{\text{cover}},(i)}_{\text{MSE}}=\left\\{\begin{array}[]{ll}(p_{x}-m^{{\text{cover}},(i)}_{\mu_{x}})^{2}+(p_{y}-m^{{\text{cover}},(i)}_{\mu_{y}})^{2}-(d^{\text{cover}})^{2}&\quad\text{if
}m^{\text{unocc},(i)}=1\\\ -(d^{\text{cover}})^{2}&\quad\text{if
}m^{\text{unocc},(i)}=0\end{array}\right.$

5: end for

Figure 5: Confidence-based risk analysis: the scene is illustrated through the
point-of-view (POV) of a third-person (top left) and robot (top middle and top
right). The aggregated pointcloud (bottom left) has regions of no returns from
the area on the left side of the robot. These holes in the pointcloud are
marked as negative obstacles in the risk layer (bottom middle) only when there
are no returns from these regions despite them being unoccluded and
sufficiently covered by laser strike pattern. This risk layer is aggregated
with the geometric risk layers to compute the final CVaR map (bottom right).

####  3.3.3 Semantics-based risk sources

LiDAR intensity returns are different on different materials at the same
distance from the sensor. In [32], the authors demonstrate the efficacy of
terrain classification using LiDAR intensity and height returns. We account
for changes in the terrain features based on changes in the intensity of LiDAR
returns. Areas that comprise a lot of mud and water have low intensity of
returns. This enables detection of muddy regions where the robot may get stuck
or fall down. Areas with deep water levels are similarly detected as negative
obstacles because the LiDAR returns are very sparse. Notably, negative
obstacles combined with low intensity of returns allows us to detect lethal
regions with deep water levels. Figure 6 shows an instance of the water-
detection when the robot is near a water puddle. Note that in Figure 5, on the
contrary, while there are holes in the LiDAR pointcloud, they are not
surrounded by low intensity returns and hence classified as negative
obstacles.

Figure 6: Semantics-based risk analysis: the robot camera (top left) shows a
puddle of water in front of the robot. The aggregated pointcloud (top right)
has regions of no returns from the area in front of the robot. These holes in
the pointcloud are marked as water in the risk layer (bottom left) when the
holes in the pointcloud are present with low intensity returns in the area
near the hole. This risk layer is aggregated with the geometric risk layers to
compute the final CVaR map (bottom middle and bottom right) with different
levels of robustness $\alpha$. We clearly see the effects on the risk map,
where higher values of $\alpha$ result assigning high cost to unknown regions.

###  3.4 Traversability Cost

To efficiently compute the Conditional Value-at-Risk (CVaR) traversability
cost for $l>1$ risk factors, we assume each risk factor $R_{l}$ is an
independent random variable which is normally distributed, with mean $\mu_{l}$
and variance $\sigma_{l}$. We take a weighted average of the risk factors to
obtain the terrain assessment model, $R=\sum_{l}w_{l}R_{l}$, which will also
be normally distributed as $R\sim\mathcal{N}(\mu,\sigma^{2})$. Let $\varphi$
and $\bm{\Phi}$ denote the probability density function and cumulative
distribution function of a standard normal distribution respectively. The
corresponding CVaR is computed as:

$\displaystyle\rho(R)=\mu+\sigma\frac{\varphi(\bm{\Phi}^{-1}(\alpha))}{1-\alpha}$
(16)

The CVaR cost accounts for the expected cost in the $(1-\alpha)$ tail of the
distribution of the terrain assessment model, $R$, thus also accounting for
high-risk, low probability events. We construct the $R$ such that the
expectation of $R$ is positive, to keep the CVaR value positive, $\mu\geq 0$.

Construction of the mean and variance of each risk factor depends on the type
of risk. For example, collision risk is determined by checking for points
above the elevation map, and the variance is derived from the elevation map
variance, which is mainly a function of localization error. In contrast,
negative obstacle risk is determined by looking for gaps in sensor
measurements. These gaps tend to be a function of sensor sparsity, so the risk
variance increases with distance from the sensor frame.

###  3.5 Risk-aware Geometric Planning

In order to optimize (8) and (9), the geometric planner computes an optimal
path that minimizes the position-dependent dynamic risk metric in (5) along
the path. Substituting (16) into (5), we obtain:

$\displaystyle J_{\mathrm{pos}}(x_{0:N})$
$\displaystyle=\mu_{0}+\sum_{k=1}^{N}\bigg{[}\mu_{k}+\sigma_{k}\frac{\varphi(\bm{\Phi}^{-1}(\alpha))}{1-\alpha}\bigg{]}$
(17)

(For a proof, see Appendix B.)

We use the A∗ algorithm to solve (8) over a 2D grid. A∗ requires a path cost
$g(n)$ and an admissible heuristic cost $h(n)$, given by:

$\displaystyle g(n)$
$\displaystyle=J_{\mathrm{pos}}(x_{0:n})+\lambda\sum_{k=0}^{n-1}\|x_{k}-x_{k+1}\|^{2},$
(18) $\displaystyle h(n)$ $\displaystyle=\lambda\,\|x_{n}-x_{G}\|^{2}.$ (19)

For the heuristic cost we use the shortest Euclidean distance to the goal,
which is always less than the actual path cost. The parameter lambda is a
relative weighting between the distance penalty and risk penalty and can be
thought of as having units of (traversability cost / m). Our experiments use a
relatively small value, which means we are mainly concerned with minimizing
traversability costs.

###  3.6 Risk-aware Kinodynamic Planning

The geometric planner produces a path, i.e. a sequence of poses. We wish to
find a kinodynamically feasible trajectory which stays near this path, while
satisfying all constraints and minimizing the CVaR cost. To solve (10)-(13),
we use a risk-aware kinodynamic MPC planner, whose steps we outline (Figure 7,
Algorithm 2, Appendix C).

Figure 7: Diagram of kinodynamic MPC planner, which begins with evaluating
various paths within a trajectory library. The lowest cost path is chosen as a
candidate and optimized by the QP solver. Algorithm 2 Kinodynamic MPC Planner
(sequences $\\{\text{var}_{k}\\}_{k=0:T}$ are expressed as $\\{\text{var}\\}$
for brevity)

0: current state $x_{0}$, current control sequence (previous solution)
$\\{u^{*}\\}^{(j)}$

0: re-planned trajectory $\\{x^{*}\\}^{(j+1)}$, re-planned control sequence
$\\{u^{*}\\}^{(j+1)}$ # Initialization

1: $\\{x^{\mathrm{r}}\\}=$ updateReferenceTrajectory()

2: $\\{u^{*}\\}^{(j)}=$ stepControlSequenceForward($\\{u^{*}\\}^{(j)}$) # Loop
process

3: for $i=0$ to qp_iterations do

4: $l$ = generateTrajectoryLibrary($x_{0}$)

5: $[\\{x^{\mathrm{c}}\\},\\{u^{\mathrm{c}}\\}]=$
chooseCandidateFromLibrary($l$)

6: $[\\{\delta x^{*}\\},\\{\delta u^{*}\\}]=$
solveQP($\\{x^{\mathrm{c}}\\},\\{u^{\mathrm{c}}\\},\\{x^{\mathrm{r}}\\}$)

7: $[\gamma,solved]=$ lineSearch($\\{x^{\mathrm{c}}\\},\\{\delta
x^{*}\\},\\{u^{\mathrm{c}}\\},\\{\delta u^{*}\\}$)

8: ${u^{c}_{k}}=u^{\mathrm{c}}_{k}+\gamma\delta u^{*}_{k},~{}\forall k=0:T$

9: $\\{x^{c}\\}=$ rollOutTrajectory($x_{0},\\{u^{c}\\}$)

10: end for

11: if solved then

12: $\\{x^{*}\\}^{(j+1)},\\{u^{*}\\}^{(j+1)}=\\{x^{c}\\},\\{u^{c}\\}$

13: else

14: $\\{x^{*}\\}^{(j+1)},\\{u^{*}\\}^{(j+1)}=$ getStoppingTrajectory()

15: end if

16: return $\\{x^{*}\\}^{(j+1)},\\{u^{*}\\}^{(j+1)}$

Trajectory library: Our kinodynamic planner begins with selecting the best
candidate trajectory from a trajectory library, which stores multiple initial
control and state sequences. The selected trajectory is used as initial
solution for solving a full optimization problem. The trajectory library can
include: 1) the trajectory accepted in the previous planning iteration, 2) a
stopping (braking) trajectory, 3) a geometric plan following trajectory, 4)
heuristically defined trajectories (including v-turns, u-turns, and varying
curvatures), and 5) randomly perturbed control input sequences.

QP Optimization: Next, we construct a non-linear optimization problem with
costs and constraints (10–13). We linearize the problem about the initial
solution and solve iteratively in a sequential quadratic programming (SQP)
fashion [41]. Let $\\{\hat{x}_{k},\hat{u}_{k}\\}_{k=0:T}$ denote an initial
solution. Let $\\{\delta x_{k},\delta u_{k}\\}_{k=0:T}$ denote deviation from
the initial solution. We introduce the solution vector variable $X$:

$X=\begin{bmatrix}\delta x_{0}^{\mathrm{T}}&\cdots&\delta
x_{T}^{\mathrm{T}}&\delta u_{0}^{\mathrm{T}}&\cdots&\delta
u_{T}^{\mathrm{T}}\\\ \end{bmatrix}^{\mathrm{T}}$ (20)

We can then write (42–45) in the form:

minimize $\displaystyle\frac{1}{2}X^{\mathrm{T}}PX+q^{\mathrm{T}}X$ (21)
subject to $\displaystyle l\leq AX\leq u$ (22)

where $P$ is a positive semi-definite weight matrix, $q$ is a vector to define
the first order term in the objective function, $A$ defines inequality
constraints and $l$ and $u$ provide their lower and upper limit. (See Appendix
E.) The next subsection (Section 3.6.1) we describe these costs and
constraints in detail. This is a quadratic program, which can be solved using
commonly available QP solvers such as our implementation uses the OSQP solver,
which is a robust and highly efficient general-purpose solver for convex QPs
[50].

Linesearch: The solution to the SQP problem returns an optimized variation of
the control sequence $\\{\delta u_{k}^{*}\\}_{k=0:T}$. We then use a
linesearch procedure to determine the amount of deviation $\gamma>0$ to add to
the current candidate control policy $\pi$: $u_{k}=u_{k}+\gamma\delta
u^{*}_{k}$. (See Appendix F.)

Stopping Sequence: If no good solution is found from the linesearch, we pick
the lowest cost trajectory from the trajectory library with no collisions. If
all trajectories are in collision, we generate an emergency stopping sequence
to slow the robot as much as possible (a collision may occur, but hopefully
with minimal energy).

Tracking Controller: Having found a feasible and CVaR-minimizing trajectory,
we send it to a tracking controller to generate closed-loop tracking behavior
at a high rate (>100Hz), which is specific to the robot type (e.g. a simple
cascaded PID, or legged locomotive controller).

####  3.6.1 Optimization Costs and Constraints

Costs: Note that the cost (10) includes the CVaR risk. To linearize the cost
for representation in a QP, we compute the Jacobian and Hessian of $\rho$ with
respect to the state $x$. We efficiently approximate these terms via numerical
differentiation.

Kinodynamic constraints: Similar to the cost, we linearize the system dynamics
(11) with respect to $x$ and $u$. Depending on the dynamics model, this may be
done analytically.

Control limits: The constraint function $g(u)$ in (12) limits the range of the
control inputs. For example in the 6-state dynamics case, we limit maximum
accelerations: $|a_{x}|<a^{\max}_{x}$, $|a_{y}|<a^{\max}_{y}$, and
$|a_{\theta}|<a^{\max}_{\theta}$.

State limits: Within the constraint function $h(m,x)$ in (13), we encode
velocity constraints: $|v_{x}|<v_{x}^{\max}$, $|v_{y}|<v_{y}^{\max}$, and
$|v_{\theta}|<v_{\theta}^{\max}$. We also constrain the velocity of the
vehicle to be less than some scalar multiple of the risk in that region, along
with maximum allowable velocities:

$\displaystyle|v_{\theta}|$ $\displaystyle<\gamma_{\theta}\,\rho(R_{k})$ (23)
$\displaystyle\sqrt{v_{x}^{2}+v_{y}^{2}}$
$\displaystyle<\gamma_{v}\,\rho(R_{k})$ (24)

This constraint reduces the robot’s kinetic energy in riskier situations,
preventing more serious damage.

Position risk constraints: The function $h(m,x_{k})$ also encodes constraints
on robot position and orientation to prevent the robot from hitting obstacles.
The general form of this constraint is:

$\displaystyle\rho(R_{k})<\rho^{\max}.$ (25)

To formulate the constraint (25), we locate areas on the map where the risk
$\rho$ is greater than the maximum allowable risk. These areas are marked as
obstacles, and are often highly non-convex. To obtain a convex and tractable
approximation of this highly non-convex constraint, we decompose obstacles
into non-overlapping 2D convex polygons, and create a signed distance function
which determines the minimum distance between the robot’s footprint (also
assumed to be a convex polygon) and each obstacle [47]. Let
$\mathcal{A},\mathcal{B}\subset\mathbb{R}^{2}$ be two convex sets, and define
the distance between them as:

$\displaystyle\mathrm{dist}(\mathcal{A},\mathcal{B})=\inf\\{\|T\|~{}|~{}(T+\mathcal{A})\cap\mathcal{B}\neq\emptyset\\}$
(26)

where $T$ is a translation. When the two sets are overlapping, define the
penetration distance as:

$\displaystyle\mathrm{penetration}(\mathcal{A},\mathcal{B})=\inf\\{\|T\|~{}|~{}(T+\mathcal{A})\cap\mathcal{B}=\emptyset\\}$
(27)

Then we can define the signed distance between the two sets as:

$\displaystyle\mathrm{sd}(\mathcal{A},\mathcal{B})=\mathrm{dist}(\mathcal{A},\mathcal{B})-\mathrm{penetration}(\mathcal{A},\mathcal{B})$
(28)

We then include within the function $h(m,x_{k})$ a constraint to enforce the
following inequality:

$\displaystyle\mathrm{sd}(\mathcal{A}_{\mathrm{robot}},\mathcal{B}_{i})>0\quad\forall
i\in\\{0,\cdots,N_{\mathrm{obstacles}}\\}.$ (29)

Note that the robot footprint $\mathcal{A}_{\mathrm{robot}}$ depends on the
current robot position and orientation:
$\mathcal{A}_{\mathrm{robot}}(p_{x},p_{y},p_{\theta})$, while each obstacle
$\mathcal{B}_{i}(m)$ is dependent on map information (See Figure 8).

Figure 8: Left: Computing convex to convex signed distance function between
the robot footprint and an obstacle. Signed distance is positive with no
intersection and negative with intersection. Right: Robot pitch and roll are
computed from the surface normal rotated by the yaw of the robot. Purple
rectangle is the robot footprint with surface normal $n^{w}$. $\bm{g}$ denotes
gravity vector, $n^{r}_{x,y,z}$ are the robot-centric surface normal
components used for computing pitch and roll.

Orientation constraints: To constrain the robot’s orientation on sloped
terrain so as to prevent the robot from rolling over or performing dangerous
maneuvers, we add constraints to the function $h(m,x_{k})$ which limit the
roll and pitch of the robot as it settles on the surface of the ground. Denote
the robot’s position as $p=[p_{x},p_{y}]^{\intercal}$ and the position/yaw as
$s=[p_{x},p_{y},p_{\theta}]^{\intercal}$. Let $\psi$ and $\phi$ denote the
robot’s pitch and roll in its body frame. Let
$\omega=[\psi,\phi]^{\intercal}$. The constraint will have the form
$|\omega|\prec\omega^{\max}$. Let the surface normal vector at point $p$ be
denoted by $n^{w}=[n^{w}_{x},n^{w}_{y},n^{w}_{z}]^{\intercal}$, in the world
frame. Let $n^{r}=[n^{r}_{x},n^{r}_{y},n^{r}_{z}]^{\intercal}$, be the surface
normal in the body frame, where we rotate by the robot’s yaw:
$n^{r}=R_{\theta}n^{w}$ (see Figure 8), where rotation matrix $R_{\theta}$
represents a rotation by the angle $\theta$ about the world $z$ axis. Then, we
define the robot pitch and roll as $\omega=g(n^{r})$ where:

$\displaystyle\omega=g(n^{r})=\begin{bmatrix}\mathrm{atan2}(n^{r}_{x},n^{r}_{z})\\\
-\mathrm{atan2}(n^{r}_{y},n^{r}_{z})\end{bmatrix}$ (30)

Creating a linearly-constrained optimization problem requires a linear
approximation of the constraint:

$|\nabla_{s}\omega(s)\delta s+\omega(s)|<\omega^{\max}$ (31)

The linearization is realized by finding the gradients of the orientation
constraints with respect to position and yaw separately (See Appendix D).

Box Constraint: Note that if $\delta x$ and $\delta u$ are too large,
linearization errors will dominate. To mitigate this effect we also include
box constraints within (12) and (13) to maintain a bounded deviation from the
initial solution: $|\delta x|<\epsilon_{x}$ and $|\delta u|<\epsilon_{u}$.

Adding Slack Variables: To further improve the feasibility of the optimization
problem we introduce auxilliary slack variables for constraints on state
limits, position risk, and orientation. For a given constraint $h(x)>0$ we
introduce the slack variable $\epsilon$, and modify the constraint to be
$h(x)>\epsilon$ and $\epsilon<0$. We then penalize large slack variables with
a quadratic cost: $\lambda_{\epsilon}\epsilon^{2}$. These slack variables are
incorporated into the QP problem (21) and (22).

###  3.7 Dynamic Risk Adjustment

The CVaR metric allows us to dynamically adjust the level and severity of risk
we are willing to accept. Selecting low $\alpha$ reverts towards using the
mean cost as a metric, leading to optimistic decision making while ignoring
low-probability but high cost events. Conversely, selecting a high $\alpha$
leans towards conservatism, reducing the likelihood of fatal events while
reducing the set of possible paths. We adjust $\alpha$ according to two
criteria: 1) Mission-level states, where depending on the robot’s role, or the
balance of environment and robot capabilities, the risk posture for individual
robots may differ. 2) Recovery Behaviors, where if the robot is trapped in an
unfavorable condition, by gradually decreasing $\alpha$, an escape plan can be
found with minimal risk. These heuristics are especially useful in the case of
risk-aware planning, because the feasibility of online nonlinear MPC is
difficult to guarantee. When no feasible solution is found for a given risk
level $\alpha$, a riskier but feasible solution can be quickly found and
executed.

###  3.8 Risk-Driven Gait Selection

We considered two platforms for testing our traversability framework - a
Clearpath Husky (wheeled robot) and Boston Dynamics Spot (quadrupedal legged
robot). In the case of the Spot, multiple locomotion gaits are available,
namely, walking gait (standard operation, most stable), stair climbing gait
(reduces the robot speed and pitches the robot to better see the stairs while
descending), crawling gait (three feet touch the ground at all times, the most
stable gait)111Further descriptions of the gaits can be found here:
https://support.bostondynamics.com/s/article/Operating-Spot. Furthermore, the
height of the robot can also be dynamically adjusted; the height reduction to
fit in small spaces is henceforth called the crouch gait. These locomotion
gaits provide mobility for the Spot in areas that would not be traversable
with the standard walking gait.

  * •

Stair gait selection: Stair detection was performed using geometric analysis
of LiDAR and camera disparity pointcloud data. To detect stairs, we use a
plane fitting method similar to [48]. We compute normals and perform
clustering to identify contiguous surfaces in the pointcloud of similar slope.
We then isolate flat and vertical planes to detect potential stair surfaces.
After finding candidate stair surfaces, the larger stair direction and overall
plane is determined. Finally, the intersection of the ground plane that the
robot currently sits on and the plane of the stairs is used to determine the
start of the stairs. Once the start of the stairs (either up or down) is
determined, the robot can align itself to this location and begin a stair
walking gait.

Figure 9: Stair detection with plane-fitting method

  * •

Crouch gait through slip detection: A learning-based slip prediction module
was implemented in [14] that leveraged the joint state information of the
quadruped at each time instant. We train an ensemble model to predict the
probability of slip, $p(slip)$, at each time instant, based on the robot limb
kinematics and kinetics. Slip-annotations from multiple field trials across
different terrains (e.g., rocky, sandy, muddy terrains) formed the ground
truth for the model training. During slip prediction using the trained model,
if the $p(slip)>0.5$, the crawling gait mode is enabled for the robot to
traverse the potentially slippery terrain. For more details on the
implementation of the slip-predictive model, see [14].

Figure 10: Gait change through slip detection

  * •

Crouch behavior in low ceiling areas: If the robot detects a region with low
ceiling that is within 16 cm of the robot height (the limit to which the robot
can bend), the robot enables crouch gait that reduces its height of the robot
by 16 cm. Once the robot passes through the low ceiling area, it switches back
to the walking gait.

Figure 11: Crouch behavior in low ceiling areas

###  3.9 Recovery Behaviors

Recovery behaviors ensure fast, reactive actions to hazardous, mission-ending
situations such that the robot can recover quickly and safely. In the case
that a recovery behavior is activated, the robot temporarily pauses following
the commands from the geometric and kinodynamic planning pipeline to get to a
safe state from where it can resume the hierarchical planning pipeline (see
Figure 2).

Tilt recovery: The tilt recovery behavior tracks the odometry received from
the state estimation pipeline for the wheeled robot. The odometry is a
combination of wheel odometry and inertial measurements. It consists of the
pose and twist measurements. If the robot’s pitch exceeds a threshold, the
robot backtracks at a low speed along the same path that got the robot in the
lethal pitch state.

Figure 12: Tilt recovery behavior when the robot pitch exceeds the safe limit.

Wiggle behavior: We monitor the robot’s velocity obtained from wheel and
inertial odometry to ensure that the robot is not stuck during mission. If the
robot does not move for a certain time period while the MPC provides robot
movement commands, the robot is considered to be stuck. In this case, we
deploy a wiggle behavior that moves the wheels of the robot in such a way
that, unintruded, the robot would travel in a figure-8 pattern. The allows the
robot to wiggle back and forth till it is not stuck anymore. If the wiggle
behavior fails to ensure recovery, the robot is considered stuck and the
mission ends.

Escape lethal: If the robot finds itself trapped in a lethal zone for some
period of time, it is useful to have a behavior which attempts to escape the
high risk regions with minimum risk. This is accomplished in the following
way. First, a goal location is chosen which is not in a lethal area, close to
the robot, and which minimizes the amount of lethal area the robot would need
to traverse to reach the goal in a straight line. Next, the risk threshold of
the traversability cost maps are changed, to allow traversal through high risk
map cells which would normally be treated as constraints or obstacles. Then,
the goal is sent to the global and local planners, and executed on the robot.
If the goal is reached, the recovery behavior is ended, but if the goal is
still not reachable, the risk threshold can be further adjusted to allow more
and more risky behavior in an attempt to escape. In this way, the CVaR risk
threshold is useful for adapting the risk profile of the robot in real-time.

##  4 Experiments

In this section, we report the experimental and field performance of STEP. We
first present a comparative study between different adjustable risk thresholds
in simulation on a wheeled differential drive platform. Then, we demonstrate
real-world performance using a wheeled robot deployed in an abandoned subway
filled with clutter, and a legged platform deployed in a lava tube
environment.

###  4.1 Simulation Study

Figure 13: Path distributions from four simulated runs. The risk level
$\alpha$ spans from 0.1 (close to mean-value) to 0.95 (conservative). Smaller
$\alpha$ typically results in a shorter path, while larger $\alpha$ chooses
statistically safe paths. Figure 14: Distance vs risk trade-off from 50 Monte-
Carlo simulations. Left: Distributions of path distance. Right: Distributions
of max risk along the traversed paths. Box plot uses standard quartile format
and dots are outliers.

To assess statistical performance, we perform 50 Monte-Carlo simulations with
randomly generated maps and goals. These results were also included in our
previous work [20]. Random traversability costs are assigned to each grid
cell. The following assumptions are made: 1) no localization error, 2) no
tracking error, and 3) a simplified perception model with artificial noise. We
give a random goal 8 m away and evaluate the path cost and distance. We use a
differential-drive dynamics model (no lateral velocity).

We compare STEP using different $\alpha$ levels. Figure 13 shows the
distribution of paths for different planning configurations. The optimistic
(close to mean-value) planner $\alpha=0.05$ typically generates shorter paths,
while the conservative setting $\alpha=0.95$ makes long detours to select
statistically safer paths. The other $\alpha$ settings show distributions
between these two extremes, with larger $\alpha$ generating similar paths to
the conservative planner and smaller $\alpha$ generating more time-optimal
paths. Statistics are shown in Figure 14.

###  4.2 Hardware Results

We deployed STEP on two different robots (wheeled and legged) in different
challenging environments (an abandoned subway, a lava tube, a limestone mine,
and a combination of different subterranean environments in the DARPA
Subterranean Challenge). The robot was equipped with custom sensing and
computing units, and driven by JPL’s NeBula autonomy software [1]. 3 Velodyne
VLP-16s were used for collecting LiDAR data. Localization was provided onboard
by a LiDAR-based SLAM solution [44, 19]. The entire autonomy stack runs on an
Intel Core i7 CPU. The typical CPU usage for the traversability stack is about
a single core.

####  4.2.1 Previous Results

Earlier versions of STEP (without semantic and confidence-aware risk sources,
gait adaptations, and the entire suite of recovery behaviors) were tested in
multiple field locations. For the sake of completeness, we have included these
results, published in [20], from:

  1. 1.

Abandoned subway filled with industrial clutter in Downtown Los Angeles, CA in
Figure 15,

  2. 2.

Valentine Cave in Lava Beds National Monument, Tulelake, CA in Figure 16

Figure 15: Traversability analysis results for Husky in an abandoned subway
experiment. Top left to right: Risk maps at three varying risk levels:
$\alpha=0.1,~{}0.5,~{}0.9$, respectively. Colors correspond to CVaR value
(white: safe ($r<=0.05$), yellow to red: moderate ($0.05<r<=0.5$), black:
risky ($r>0.5$)). Also shown are the most recent LiDAR measurements (green
points). Bottom left and middle: Front and right on-board cameras observing
the same location. Bottom right: Completed top-down map of the environment
after autonomous exploration. Bright dots are pillars, which are visible in
the camera images. The effect of these risk analyses results in intuitive
outcomes - for example, a low pile of metal, while probably traversable,
should be avoided if possible. When the region has inadequate sensor coverage,
the risk will be high. When the robot is closer and the sensor coverage is
good, then the CVaR cost will decrease, yielding a more accurate risk
assessment. This results in more efficient and safer planning when compared to
deterministic methods. Figure 16: Traversability analysis results for the
Valentine Cave experiment. From left to right: Third-person view, elevation
map (colored by normal direction), risk map (colored by risk level. white:
safe ($r<=0.05$), yellow to red: moderate ($0.05<r<=0.5$), black: risky
($r>0.5$)), and planned geometric/kinodynamic paths (yellow lines/red boxes).

####  4.2.2 Results from the Kentucky Underground and LA Subway

We tested the various new parts of our traversability pipeline during fully
autonomous runs in our tests at Kentucky Underground Storage (KU), Wilmore,
KY. The results of the cost map for confidence-based and semantics-based risk
are seen in Figure 5 and Figure 6. We also tested the tilt recovery behavior
on a pile of rubble in KU by forcing the robot into a hazardous tilt position,
the robot was able to recover from this risky configuration as seen in Figure
12.

We also tested new results in an abandoned subway with multiple levels filled
with industrial clutter in Downtown Los Angeles, CA. For example, the LA
Subway envirnoment served as a test bed for the stair gait and crouch gait
behaviors that are shown in Figure 9 and Figure 11 respectively.

####  4.2.3 Results from the DARPA Subterranean Challenge

The traversability framework was deployed in the DARPA Subterranean Challenge
held in Louisville Mega Cavern, Louisville, KY. The course consisted of 3
different environments - tunnel, urban, and cave. Figure 17 shows the course
map and different sections that the robot explored. The sections showcased in
Figure 17 have a high difficulty rating as listed in the course layout guide
posted by DARPA222The course guide can be accessed here:
https://bitbucket.org/subtchallenge/finals_ground_truth/src/master/course_design/Finals_Course_Callouts.pdf.
The terrain challenges of each of the sections are listed below:

Region A

An office-like area that consists of narrow corridors ($5^{\prime}\times
8^{\prime}$) and small rooms ($8^{\prime}\times 8^{\prime}$) for the robots to
explore.

Region B

A warehouse-like area with a lot of shelving and clutter imitating an
industrial warehouse after an earthquake.

Region C

A connection between the urban and tunnel part of the course. The stairs act
as a negative obstacle for the wheeled robots. The presence of this negative
obstacle in a narrow corridor makes the drop harder to detect.

Region D

A constrained passage with ground, wall, and ceiling obstacles - with vertical
pipes and debris.

Region E

A narrow cave opening mimicking a region that humans have to crawl through.
The ground slopes upwards and the ceiling slopes downwards creating issues for
ground segmentation and low-ceiling detection.

Region F

A small limestone cave with rubble and loose rock piles.

Figure 18 shows the results of the traversability analysis and the geometric
and kinodynamic plans of the robot in the different scenarios listed above. We
see that the robot is able to plan safe paths in all of these scenarios and
successfully navigate the different terrain hazards.

Figure 17: The layout of the competition course with snapshots of the systems
build of the areas traversed by the robots that have interesting terrain and
high difficulty.

Figure 18: Traversability analysis results for the DARPA Subterranean
Challenge. From left to right: robot front camera view, elevation map (colored
by normal direction), risk map (colored by risk level- white: safe
($r<=0.05$), yellow to red: moderate ($0.05<r<=0.5$), black: risky ($r>0.5$)),
and planned geometric/kinodynamic paths (yellow lines/red boxes).

##  5 Discussion

We presented our approach to Stochastic Traversability Evaluation and Planning
and the performance of our method in the DARPA Subterranean Challenge. The
method was successfully able to account for uncertainty in the apriori unknown
environment and navigate various types of terrain. In this extension of [20],
we accounted for semantic and confidence-aware risk sources in addition to the
previously presented geometric risk sources. Additionally, we allowed for
changes in gaits of quadrupedal robots based on the terrain risk observed and
information obtained from priprioceptive sensors. We added a large suite of
recovery behaviors to robustify our traversability stack in case of
unaccounted for but mission-ending situations that the rest of the framework
cannot account for.

There are many avenues of improvement of our framework. STEP accounts for
traversability risk arising from static environments, but it does not make
predictions for dynamic obstacles. Hence, the robot cannot plan reactive
motions to avoid falling debris or account for sudden movement of rubble under
the robot. The traversability analysis is also the same for all speeds of the
robot. Dynamic traversability analysis will allow for a larger range of
motion. The LIDAR intensity-based semantic terrain analysis provides us with a
coarse classification of muddy terrain vs regular terrain but using a vision-
based semantic classification in addition to the LIDAR intensity information
will provide a richer understanding of the surface over which the robot is
traversing.

## Acknowledgement

The work is performed at the Jet Propulsion Laboratory, California Institute
of Technology, under a contract with the National Aeronautics and Space
Administration (80NM0018D0004), and Defense Advanced Research Projects Agency
(DARPA).

## References

  * [1] A. Agha-mohammadi, et al.  “NeBula: Quest for robotic autonomy in challenging environments; TEAM CoSTAR at the DARPA Subterranean Challenge”  In _Field Robotics_ , 2021
  * [2] A. Agha-Mohammadi, Eric Heiden, Karol Hausman and Gaurav Sukhatme  “Confidence-rich grid mapping”  In _The International Journal of Robotics Research_ 38.12-13, 2017, pp. 1352–1374
  * [3] Mohamadreza Ahmadi et al.  “Constrained Risk-Averse Markov Decision Processes”  In _AAAI Conference on Artificial Intelligence_ , 2021
  * [4] Mohamadreza Ahmadi et al.  “Risk-Averse Planning Under Uncertainty”  In _American Control Conference_ , 2020, pp. 3305–3312
  * [5] Mohamadreza Ahmadi, Anushri Dixit, Joel W. Burdick and Aaron D. Ames  “Risk-Averse Stochastic Shortest Path Planning”  In _arXiv:2103.14727_ , 2021
  * [6] A. Ahmadi-Javid  “Entropic value-at-risk: A new coherent risk measure”  In _Journal of Optimization Theory and Applications_ 155.3 Springer, 2012, pp. 1105–1123
  * [7] Philippe Artzner, Freddy Delbaen, J. Eber and David Heath  “Coherent measures of risk”  In _Mathematical finance_ 9.3 Wiley Online Library, 1999, pp. 203–228
  * [8] F. Augugliaro, A.P Schoellig and R. D’Andrea  “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach”  In _Int. Conf Intell. Robots Syst._ , 2012
  * [9] Paul T Boggs and Jon W Tolle  “Sequential quadratic programming”  In _Acta numerica_ , 1995, pp. 529–562
  * [10] Xiaoyi Cai, Michael Everett, Jonathan Fink and Jonathan P How  “Risk-Aware Off-Road Navigation via a Learned Speed Distribution Map”  In _arXiv preprint arXiv:2203.13429_ , 2022
  * [11] Eduardo F Camacho and Carlos Bordons Alba  “Model predictive control”  Springer Science & Business Media, 2013
  * [12] Yinlam Chow, Aviv Tamar, Shie Mannor and Marco Pavone  “Risk-sensitive and robust decision-making: a CVaR optimization approach”  In _Advances in Neural Information Processing Systems_ , 2015, pp. 1522–1530
  * [13] Will Dabney, Mark Rowland, Marc G Bellemare and R mi Munos  “Distributional reinforcement learning with quantile regression”  In _Thirty-Second AAAI Conference on Artificial Intelligence_ , 2018
  * [14] Sharmita Dey et al.  “PrePARE: Predictive Proprioception for Agile Failure Event Detection in Robotic Exploration of Extreme Terrains”  In _arXiv preprint arXiv:2208.00322_ , 2022
  * [15] Anushri Dixit, Mohamadreza Ahmadi and Joel W Burdick  “Distributionally Robust Model Predictive Control with Total Variation Distance”  In _arXiv preprint arXiv:2203.12062_ , 2022
  * [16] Anushri Dixit, Mohamadreza Ahmadi and Joel W. Burdick  “Distributionally Robust Model Predictive Control With Total Variation Distance”  In _IEEE Control Systems Letters_ 6, 2022, pp. 3325–3330  DOI: 10.1109/LCSYS.2022.3184921
  * [17] Anushri Dixit, Mohamadreza Ahmadi and Joel W. Burdick  “Risk-Averse Receding Horizon Motion Planning”, 2022  arXiv:2204.09596 [eess.SY]
  * [18] Anushri Dixit, Mohamadreza Ahmadi and Joel W. Burdick  “Risk-Sensitive Motion Planning using Entropic Value-at-Risk”  In _European Control Conference_ , 2021
  * [19] K Ebadi et al.  “LAMP: Large-scale autonomous mapping and positioning for exploration of perceptually-degraded subterranean environments”  In _IEEE International Conference on Robotics and Automation_ , 2020, pp. 80–86
  * [20] D.D. Fan et al.  “STEP: Stochastic Traversability Evaluation and Planning for Risk-Aware Off-road Navigation”  In _Robot.: Sci. Syst._ , 2021
  * [21] David D Fan, Ali-akbar Agha-mohammadi and Evangelos A Theodorou  “Deep learning tubes for Tube MPC”  In _Robotics: Science and Systems (RSS)_ , 2020
  * [22] David D. Fan, Ali-akbar Agha-mohammadi and Evangelos A. Theodorou  “Learning Risk-Aware Costmaps for Traversability in Challenging Environments”  In _IEEE Robotics and Automation Letters_ 7.1, 2022, pp. 279–286  DOI: 10.1109/LRA.2021.3125047
  * [23] P ter Fankhauser, Michael Bloesch and Marco Hutter  “Probabilistic Terrain Mapping for Mobile Robots With Uncertain Localization”  In _IEEE Robotics and Automation Letters_ 3.4, 2018, pp. 3019–3026
  * [24] P ter Fankhauser and Marco Hutter  “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation”  In _Robot Operating System (ROS), The Complete Reference_ , 2016, pp. 99–120
  * [25] R. Foust, S.J. Chung and F.Y. Hadaegh  “Optimal Guidance and Control with Nonlinear Dynamics Using Sequential Convex Programming”  In _J. of Guid., Control, and Dyn._ 43.4, 2020, pp. 633–644
  * [26] Lu Gan, Jessy W Grizzle, Ryan M Eustice and Maani Ghaffari  “Energy-Based Legged Robots Terrain Traversability Modeling via Deep Inverse Reinforcement Learning”  In _IEEE Robotics and Automation Letters_ 7.4 IEEE, 2022, pp. 8807–8814
  * [27] S. Ghosh, K. Otsu and M. Ono  “Probabilistic Kinematic State Estimation for Motion Planning of Planetary Rovers”  In _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2018, pp. 5148–5154
  * [28] Steven B Goldberg, Mark W Maimone and Larry Matthies  “Stereo vision and rover navigation software for planetary exploration”  In _IEEE Aerospace Conference_ , 2002  IEEE
  * [29] Alain Ha t, Thierry Simeon and Michel Ta x  “Algorithms for rough terrain trajectory planning”  In _Advanced Robotics_ 16.8 Taylor & Francis, 2002, pp. 673–699
  * [30] Astghik Hakobyan, Gyeong Chan Kim and Insoon Yang  “Risk-aware motion planning and control using CVaR-constrained optimization”  In _IEEE Robotics and Automation Letters_ 4.4 IEEE, 2019, pp. 3924–3931
  * [31] Michael Himmelsbach, Felix V Hundelshausen and H-J Wuensche  “Fast segmentation of 3d point clouds for ground vehicles”  In _Intelligent Vehicles Symposium (IV), 2010 IEEE_ , 2010, pp. 560–565  IEEE
  * [32] Li Hui, Liping Di, Huang Xianfeng and Li Deren  “Laser Intensity Used in Classification of Lidar Point Cloud Data”  In _IGARSS 2008 - 2008 IEEE International Geoscience and Remote Sensing Symposium_ 2, 2008, pp. II-1140-II–1143  DOI: 10.1109/IGARSS.2008.4779201
  * [33] Mrinal Kalakrishnan et al.  “STOMP: Stochastic trajectory optimization for motion planning”  In _IEEE International Conference on Robotics and Automation_ , 2011, pp. 4569–4574
  * [34] Himangshu Kalita, Steven Morad, Aaditya Ravindran and Jekan Thangavelautham  “Path planning and navigation inside off-world lava tubes and caves”  In _IEEE/ION Position, Location and Navigation Symposium_ , 2018, pp. 1311–1318
  * [35] Sven Koenig and Reid G Simmons  “Risk-sensitive planning with probabilistic decision graphs”  In _Principles of Knowledge Representation and Reasoning_ , 1994, pp. 363–373  Elsevier
  * [36] Thomas Lew, Riccardo Bonalli and Marco Pavone  “Chance-constrained sequential convex programming for robust trajectory optimization”  In _2020 European Control Conference (ECC)_ , 2020, pp. 1871–1878  IEEE
  * [37] Johan L fberg  “Oops! I cannot do it again: Testing for recursive feasibility in MPC”  In _Automatica_ 48.3 Elsevier, 2012, pp. 550–555
  * [38] Anirudha Majumdar and Marco Pavone  “How should a robot assess risk? Towards an axiomatic theory of risk in robotics”  In _Robotics Research_ Springer, 2020, pp. 75–84
  * [39] Travis Manderson, Stefan Wapnick, David Meger and Gregory Dudek  “Learning to drive off road on smooth terrain in unstructured environments using an on-board camera and sparse aerial images”  In _2020 IEEE International Conference on Robotics and Automation (ICRA)_ , 2020, pp. 1263–1269  IEEE
  * [40] D. Morgan, S.J. Chung and F. Hadaegh  “Model Predictive Control of Swarms of Spacecraft Using Sequential Convex Programming”  In _Journal of Guidance, Control, and Dynamics_ 37, 2014, pp. 1–16  DOI: 10.2514/1.G000218
  * [41] Jorge Nocedal and Stephen Wright  “Numerical optimization”  Springer Science & Business Media, 2006
  * [42] Masahiro Ono, Marco Pavone, Kuwata Kuwata and J.(Bob) Balaram  “Chance-constrained dynamic programming with application to risk-aware robotic space exploration”  In _Autonomous Robots_ 39.4, 2015, pp. 555–571
  * [43] Kyohei Otsu et al.  “Fast approximate clearance evaluation for rovers with articulated suspension systems”  In _Journal of Field Robotics_ 37 Wiley Online Library, 2020, pp. 768–785
  * [44] Matteo Palieri et al.  “LOCUS: A multi-sensor lidar-centric solution for high-precision odometry and 3D mapping in real-time”  In _IEEE Robotics and Automation Letters_ 6.2 IEEE, 2020, pp. 421–428
  * [45] Panagiotis Papadakis  “Terrain traversability analysis methods for unmanned ground vehicles: A survey”  In _Engineering Applications of Artificial Intelligence_ 26.4, 2013, pp. 1373–1385
  * [46] Andrzej Ruszczynski  “Risk-averse dynamic programming for Markov decision processes”  In _Mathematical Programming_ 75.2, 2014, pp. 235–261
  * [47] John Schulman et al.  “Motion planning with sequential convex optimization and convex collision checking”  In _The International Journal of Robotics Research_ 33.9 SAGE Publications Sage UK: London, England, 2014, pp. 1251–1270
  * [48] Tobias Schwarze and Zhichao Zhong  “Stair detection and tracking from egocentric stereo vision”  In _2015 IEEE International Conference on Image Processing (ICIP)_ , 2015, pp. 2690–2694  IEEE
  * [49] Sumeet Singh, Y. Chow, Anirudha Majumdar and Marco Pavone  “A Framework for Time-Consistent, Risk-Sensitive Model Predictive Control: Theory and Algorithms”  In _IEEE Transactions on Automatic Control_ 64.7, 2018, pp. 2905–2912
  * [50] B. Stellato et al.  “OSQP: an operator splitting solver for quadratic programs”  In _Mathematical Programming Computation_ , 2020, pp. 1–36
  * [51] Rohan Thakker et al.  “Autonomous off-road navigation over extreme terrains with perceptually-challenging conditions”  In _International Symposium on Experimental Robotics_ , 2020
  * [52] Allen Wang, Ashkan Jasour and Brian Williams  “Non-gaussian chance-constrained trajectory planning for autonomous vehicles under agent uncertainty”  In _Robotics and Automation Letters_ 5.4 IEEE, 2020, pp. 6041–6048
  * [53] Skylar X Wei, Anushri Dixit, Shashank Tomar and Joel W Burdick  “Moving obstacle avoidance: a data-driven risk-aware approach”  In _IEEE Control Systems Letters_ IEEE, 2022
  * [54] Huan Xu and Shie Mannor  “Distributionally robust Markov decision processes”  In _Advances in Neural Information Processing Systems_ , 2010, pp. 2505–2513

##  Appendix A Dynamics model for differential drive

For a simple system which produces forward velocity and steering, (e.g.
differential drive systems), we may wish to specify the state and controls as:

$\displaystyle x=[p_{x},p_{y},p_{\theta},v_{x}]^{\intercal}$ (32)
$\displaystyle u=[a_{x},v_{\theta}]^{\intercal}$ (33)

For example, the dynamics $x_{k+1}=f(x_{k},u_{k})$ for a simple differential-
drive system can be written as:

$\displaystyle x_{k+1}=x_{k}+\Delta t\begin{bmatrix}v_{x}\cos(p_{\theta})\\\
v_{x}\sin(p_{\theta})\\\ \gamma v_{x}+(1-\gamma)v_{\theta}\\\
a_{x}\end{bmatrix}$ (34)

where $\gamma\in[0,1]$ is a constant which adjusts the amount of turning-in-
place the vehicle is permitted.

##  Appendix B Dynamic risk metric using CVaR for Normal distributions

$\displaystyle J(x_{0},\pi)$
$\displaystyle=R_{0}+\rho_{0}\big{(}R_{1}+\rho_{1}\big{(}R_{2}+\dotsc+\rho_{T-1}(R_{T})\big{)}\big{)}$
$\displaystyle=R_{0}+\rho\Bigg{(}R_{1}+\rho\Big{(}R_{2}+\dotsc+\rho(R_{T-1}+$
$\displaystyle\qquad\mu_{T}+\sigma_{T}\frac{\varphi(\bm{\Phi}^{-1}(\alpha))}{1-\alpha})\Big{)}\Bigg{)}$
$\displaystyle=R_{0}+\rho\Bigg{(}R_{1}+\rho\Big{(}R_{2}+\dotsc+\rho(R_{T-2}+$
$\displaystyle\qquad\mu_{T-1}+\mu_{T}+(\sigma_{T-1}+\sigma_{T})\frac{\varphi(\bm{\Phi}^{-1}(\alpha))}{1-\alpha})\Big{)}\Bigg{)}$
$\displaystyle\vdots$
$\displaystyle=R_{0}+\sum_{i=1}^{T}\Bigg{(}\mu_{i}+\sigma_{i}\frac{\varphi(\bm{\Phi}^{-1}(\alpha))}{1-\alpha}\Bigg{)}$
$\displaystyle=\sum_{i=0}^{T}\rho(R_{i})$

##  Appendix C Kinodynamic Planning Diagram

See Figure 19.

Figure 19: Diagram of kinodynamic MPC planner, which begins with evaluating
various paths within a trajectory library. The lowest cost path is chosen as a
candidate and optimized by the QP solver.

##  Appendix D Gradients for Orientation Constraint

We describe in further detail the derivation of the orientation constraints.
Denote the position as $p=[p_{x},p_{y}]^{\intercal}$ and the position/yaw as
$s=[p_{x},p_{y},p_{\theta}]$. We wish to find the robot’s pitch $\psi$ and
roll $\phi$ in its body frame. Let $\omega=[\psi,\phi]^{\intercal}$. The
constraint will have the form $|\omega(s)|<=\omega_{max}$. At $p$, we compute
the surface normal vector, call it
$n^{w}=[n^{w}_{x},n^{w}_{y},n^{w}_{z}]^{\intercal}$, in the world frame. To
convert the normal vector in the body frame,
$n^{r}=[n^{r}_{x},n^{r}_{y},n^{r}_{z}]^{\intercal}$, we rotate by the robot’s
yaw: $n^{r}=R_{\theta}n^{w}$ (see Figure 8), where $R_{\theta}$ is a basic
rotation matrix by the angle $\theta$ about the world $z$ axis:

$R_{\theta}=\begin{bmatrix}\cos{p_{\theta}}&&\sin{p_{\theta}}&&0\\\
-\sin{p_{\theta}}&&\cos{p_{\theta}}&&0\\\ 0&&0&&1\end{bmatrix}$ (35)

Let the robot pitch and roll vector $\omega$ be defined as $\omega=g(n^{r})$,
where:

$\displaystyle\omega=g(n^{r})=\begin{bmatrix}\mathrm{atan2}(n^{r}_{x},n^{r}_{z})\\\
-\mathrm{atan2}(n^{r}_{y},n^{r}_{z})\end{bmatrix}$ (36)

Creating a linearly-constrained problem requires a linear approximation of the
constraint:

$|\nabla_{s}\omega(s)\delta s+\omega(s)|<=\omega_{max}$ (37)

Conveniently, computing $\nabla_{s}\omega(s)$ reduces to finding gradients
w.r.t position and yaw separately. Let
$\nabla_{s}\omega(s)=[\nabla_{p}\omega(s),\nabla_{\theta}\omega(s)]^{\intercal}$,
then:

$\displaystyle\nabla_{p}\omega(s)$
$\displaystyle=(\nabla_{n^{r}}g)(R_{\theta})(\nabla_{p}n^{w})$ (38)
$\displaystyle\nabla_{\theta}\omega(s)$
$\displaystyle=(\nabla_{n^{r}}g)(\frac{d}{d\theta}R_{\theta})(n^{w})$ (39)

where:

$\nabla_{n^{r}}g=\begin{bmatrix}\frac{n^{r}_{z}}{(n^{r}_{x})^{2}+(n^{r}_{z})^{2}}&&0&&\frac{-n^{r}_{x}}{(n^{r}_{x})^{2}+(n^{r}_{z})^{2}}\\\
0&&\frac{-n^{r}_{z}}{(n^{r}_{y})^{2}+(n^{r}_{z})^{2}}&&\frac{n^{r}_{y}}{(n^{r}_{y})^{2}+(n^{r}_{z})^{2}}\end{bmatrix}$
(40)

and

$\nabla_{p}n^{w}=\begin{bmatrix}\frac{\partial n^{w}_{x}}{\partial
p_{x}}&&\frac{\partial n^{w}_{x}}{\partial p_{y}}\\\ \frac{\partial
n^{w}_{y}}{\partial p_{x}}&&\frac{\partial n^{w}_{y}}{\partial p_{y}}\\\
\frac{\partial n^{w}_{z}}{\partial p_{x}}&&\frac{\partial n^{w}_{z}}{\partial
p_{y}}\\\ \end{bmatrix}$ (41)

The terms with the form $\frac{\partial n^{w}_{x}}{\partial p_{x}}$ amount to
computing a second-order gradient of the elevation on the 2.5D map. This can
be done efficiently with numerical methods [24].

##  Appendix E Converting non-linear MPC problem to a QP problem

Our MPC problem stated in Equations (10-13) is non-linear. In order to
efficiently find a solution we linearize the problem about an initial
solution, and solve iteratively, in a sequential quadratic programming (SQP)
fashion [41]. Let $\\{\hat{x}_{k},\hat{u}_{k}\\}_{k=0,\cdots,T}$ denote an
initial solution. Let $\\{\delta x_{k},\delta u_{k}\\}_{k=0,\cdots,T}$ denote
deviation from the initial solution. We approximate (10-13) by a problem with
quadratic costs and linear constraints with respect to $\\{\delta x,\delta
u\\}$:

$\displaystyle\\{\delta x^{*},\delta u^{*}\\}$
$\displaystyle=\operatorname*{arg\,min}_{\delta x,\delta
u}\sum_{k=0}^{T}\|\hat{x}_{k}+\delta x_{k}-x^{*}_{k}\|_{Q_{k}}$
$\displaystyle\qquad\qquad\qquad+\lambda J(\hat{x}_{k}+\delta
x_{k},\hat{u}_{k}+\delta u_{k})$ (42) $\displaystyle s.t.\quad\forall k$
$\displaystyle\in[0,\cdots,T]:$ $\displaystyle\hat{x}_{k+1}+\delta x_{k+1}$
$\displaystyle=f(\hat{x}_{k},\hat{u}_{k})+\nabla_{x}f\cdot\delta
x_{k}+\nabla_{u}f\cdot\delta u_{k}$ (43) $\displaystyle g(\hat{u_{k}})$
$\displaystyle+\nabla_{u}g\cdot\delta u_{k}\succ 0$ (44) $\displaystyle
h(m,\hat{x}_{k})$ $\displaystyle+\nabla_{x}h\cdot\delta x_{k}\succ 0$ (45)

where $J(\hat{x}_{k}+\delta x_{k},\hat{u}_{k}+\delta u_{k})$ can be
approximated with a second-order Taylor approximation (for now, assume no
dependence on controls):

$\displaystyle J(\hat{x}+\delta x)\approx J(\hat{x})+\nabla_{x}J\cdot\delta
x+\delta x^{\intercal}H(J)\delta x$ (46)

and $H(\cdot)$ denotes the Hessian. The problem is now a quadratic program
(QP) with quadratic costs and linear constraints. To solve Equations (42-45),
we introduce the solution vector variable $X$:

$X=\begin{bmatrix}\delta x_{0}^{\mathrm{T}}&\cdots&\delta
x_{T}^{\mathrm{T}}&\delta u_{0}^{\mathrm{T}}&\cdots&\delta
u_{T}^{\mathrm{T}}\\\ \end{bmatrix}^{\mathrm{T}}$ (47)

We can then write Equations (42-45) in the form:

minimize $\displaystyle\frac{1}{2}X^{\mathrm{T}}PX+q^{\mathrm{T}}X$ (48)
subject to $\displaystyle l\leq AX\leq u$ (49)

where $P$ is a positive semi-definite weight matrix, $q$ is a vector to define
the first order term in the objective function, $A$ defines inequality
constraints and $l$ and $u$ provide their lower and upper limit.

##  Appendix F Linesearch Algorithm for SQP solution refinement

The solution to the SQP problem returns an optimized control sequence
$\\{u_{k}^{*}\\}_{k=0:T}$. We then use a linesearch routine to find an
appropriate correction coefficient $\gamma$, using Algorithm 3. The resulting
correction coefficient is carried over into the next path-planning loop.

Algorithm 3 Linesearch Algorithm

0: candidate control sequence $\\{u^{\mathrm{c}}_{k}\\}_{k=0:T}$, QP solution
$\\{\delta u^{*}_{k}\\}_{k=0:T}$

0: correction coefficient $\gamma$ Initialization

1: initialize $\gamma$ by default value or last-used value

2: $[c,o]=$getCostAndObstacles($\\{u^{\mathrm{c}}_{k}\\}_{k=0:T}$) Linesearch
Loop

3: for $i=0$ to max_iteration do

4: for $k=0$ to $T$ do

5: $u^{\mathrm{c}(i)}_{k}=u^{\mathrm{c}}_{k}+\gamma\delta u^{*}_{k}$

6: end for

7:
$[c^{(i)},o^{(i)}]=$getCostAndObstacles($\\{u^{\mathrm{c}(i)}_{k}\\}_{k=0:T}$)

8: if ($c^{(i)}\leq c$ and $o^{(i)}\leq o$) then

9: $\gamma=\min(2\gamma,\gamma_{max})$

10: break

11: else

12: $\gamma=\max(\gamma/2,\gamma_{min})$

13: end if

14: end for

15: return $\gamma$