Learning Risk-aware Costmaps for Traversability in Challenging Environments

A. Traversability as a random variable

We define traversability as the capability for a ground vehicle to reside over a terrain region under an admissible state [22]. We represent traversability as scalar value r which indicates the magnitude of damage (or cost of repair) the robot will experience if placed in that state. We then wish to construct a model of this traversability given the robot’s state and knowledge:

where $x\in \mathcal{X}$ is the robot position in 2-D coordinates $m\in \mathcal{M}$ represents the current belief of the local environment, and $\mathcal{R}(\cdot )$ is a traversability assessment model. This model captures various unfavorable events such as collision, getting stuck, tipping over, high slippage, to name a few. A mobility platform has a unique assessment model which reflects its mobility capability. In most real-world applications where perception capabilities are limited, 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}$, where z_0:k are sensor observations. Then, the traversability estimate is a random variable $R:(\mathcal{M}\times \mathcal{X})\to ℝ$. We call this probabilistic mapping from map belief and state to possible traversability cost values a risk assessment model.

B. Risk Metrics, VaR and CVaR

A risk metric $\rho (R):R\to ℝ$ is a mapping from a random variable to a real number which quantifies some notion of risk. Local environment information is encoded within m. This can be raw sensor data, e.g. observations, or processed map data (e.g. represented as m=(m⁽¹⁾,m⁽²⁾, ⋯) where mⁱ is the i-th element of the map). In the case of a 2-D or 2.5-D representation, i represents the cell index. Risk metrics can be coherent, which mean they satisfy six properties with respect to the random variables they act upon, namely: normalized, monotonic, sub-additive, positive homogeneity, and translation invariant. Majumdar and Pavone [20] argues that the consideration of risk in robotics should utilize coherent risk metrics for their intuitive and well-formulated properties. In this work we are concerned with right-tail risk only, since the random variable R is a positive “cost” for which we seek to avoid high values.

Let α ∈ [0,1] denote the risk probability level. High values of α imply more risk. The value-at-risk of the random variable R(m,x) can be defined by (we write R for brevity):

This is also known as the α-quantile. While there are multiple ways to define the conditional value-at-risk, one common definition is as follows:

C. Learning VaR and CVaR

We wish to construct a model with inputs m, x, and α, and outputs CVaR_α(R(m, x)), parameterized by network weights θ. We denote this model as C_θ(m, x, α). To learn CVaR we take a joint approach where we learn both VaR and CVaR together. We construct a similar VaR model, also as a function of m and x as VaR_α(R(m, x)), which we denote V_θ(m, x, α). Learning VaR can be accomplished by minimizing the Koenker-Bassett error with respect to θ [15]:

where (·)₊ = max(·, 0) and (·)₋ = min(·, 0). From the estimated VaR, we can compute the expected CVaR and construct an L1-loss for C_θ:

With the assumption of i.i.d. sampled data, when $l_{\alpha }^V(\theta )$ and $l_{\alpha }^C(\theta )$ are minimized, V_θ will approximate VaR and C_θ will approximate CVaR. If during training we uniformly randomly sample values of α ∈ [0,1], then the input α to these models should be meaningful as well.

In practice, a few modifications to these losses are needed to improve numerical stability. First, similar to [4], we smooth the quantile loss function $l_{\alpha }^V$ near the inflection point V_θ=R by using a modified Huber loss [10]:

where $h\in {ℝ}^{+}$ controls how much smoothing is added (Figure 3). The new VaR loss is then:

Second, instead of learning C_θ directly, we can learn the residual between C_θ and V_θ. Suppose we construct a model ${\widehat{C}}_{\theta }$ instead of C_θ, and let $C_{\theta }=V_{\theta }+{\widehat{C}}_{\theta }$. Then the loss $l_{\alpha }^C(\theta )$ can be written as:

This serves to separate the error signals between the two models.

Third, we introduce a monotonic loss to enforce that increasing values of α result in increasing values of V_θ(m,x,α) and C_θ(m,x,α). This can be done by penalizing negative divergence of the output with respect to α [6]. We also introduce a smoothing function to prevent instability near the inflection point when the divergence equals 0. The total monotonic loss is:

In practice we find that under gradient-based optimization, this loss decreases to near 0 in the first epoch and does not notice-ably affect the minimization of the quantile and CVaR losses.

To summarize, the total loss function is the sum of the modified Huber quantile loss (7), the L1 residual CVaR loss (8), and the monotonic loss (12):

We minimize this loss over a dataset, sampled i.i.d from the distribution of R, which is a function of m and x. We also sample from a uniform distribution for α. In other words, we seek to minimize the expected loss with respect to θ:

This expectation is approximated using stochastic gradient descent [26]. Given the dataset $\mathcal{D}={\left\{m_n,x_n\right\}}_n$, the distribution p(m, x) is sampled from the distribution of data collected by the robot as it moves in the environment, observing samples of its position x and its environment data m. The distribution p(R|m,x) is sampled by determining the traversability cost given a sampled (m,x). This determination may be stochastic or imprecise, and may possibly come from any traversability assessment method. Happily, our learning approach should capture this stochasticity. In Figure 4, we demonstrate this learning approach on a toy 1-D problem. We use a 3 layer feed-forward MLP neural network to learn the state-varying VaR and CVaR values of a random variable. We notice that our learning approach captures the risk of the known distributions accurately. Next, we describe the application of this loss function to learning unknown distributions of 2-D traversability costs from 3D pointclouds.

D. Obtaining Ground Truth Labels

As mentioned, computing traversability costs from sensor data is a rich field in itself. Computing these costs may include geometric analysis [8], semantic understanding and detection [17], proprioceptive sensing [18], or hand-labeling. Our approach is extensible to both dense and sparse ground truth labels in the costmap. In this work we focus on costs arising through geometric analysis. We leverage prior work (Fan et al. [7]), which itself follows a tradition of geometric analyses for ground vehicle traversal [21, 8]. These analyses are based on pointcloud data and provide model-based costs which are computed in a dense local region around the robot, but are affected by sensor occlusion, sparsity, noise, and localization error. These sources of ambiguity and stochasticity are exactly what we aim to capture with our risk-aware model. However, instead of having to explicitly model these uncertainties, we simple compute a mean value best-guess at the traversability cost, and use learning to aggregate the variation of these costs.

A. Dataset

Our motivation for tackling this problem comes from extensive field testing in various underground caves and decaying urban environments [1]. We collected data from autonomous exploration runs in six different environments of various types (Figure 5, Table I). These environments are as follows:

• An abandoned subway station in Los Angeles, CA, the first floor has a large open atrium with pillars (Subway Atrium / SA).

• The basement floor of the same subway station, featuring offices and narrow corridors (Subway Office / SO).

• Kentucky Underground Storage in Lexington, KY, which consists of very large cavernous grids of tunnels (Limestone Mine / LM)

• Wells Cave in Pulaski County, KY, which is a natural limestone cave with very rough floors, narrow passages, low ceilings, and rubble (Limestone Cave / LC)

• Valentine Cave at Lava Beds National Park, CA, which is an naturally formed ancient lava tube, with sloping walls and rough, rocky floors (Lava Tube A / TA)

• Mammoth Cave at Lava Beds National Park, CA, which is also a lava tube but with sandy, sloping floors and occasional large piles of rubble (Lava Tube B / TB).

Data was collected using Boston Dynamics’s Spot legged robot equipped with JPL’s NeBula payload [1]. The payload includes onboard computing, one VLP-16 Velodyne LiDAR sensor, and a range of other cameras and sensors. Spot is equipped with 5 Intel RealSense depth cameras which are pointed at the ground. A new data sample was added to the dataset at approximately 0.5Hz, while the average top speed of the robot was set to 1m/s.

B. Computing Traversability Cost

Traversability costs are computed online and saved along with dataset. These costs are generated via a risk-aware traversability pipeline [7], which we will briefly summarize here. First, we aggregate LiDAR pointclouds over time, using odometry. Odometry is generated using a combination of Spot’s on-board VIO and ICP-based pointcloud matching. Next, we perform ground segmentation to isolate “ground” points, “obstacle” points, and “ceiling” points. We then perform elevation mapping, which probabilistically builds a 2.5-D elevation map using the “ground” points. Localization noise and delay are taken into account here, resulting in elevation variance information as well. We then compute costs as a function of various geometric information arising from the elevation map and the pointcloud, such as slope, step size, roughness, negative obstacles, and collision obstacles. These costs are then aggregated into a combined mean cost (as well as a cost variance). The traversability cost is scaled between 0 and 1, with 1 being lethal and 0 being safe. In [7], the mean cost and variance of the cost are then used to compute CVaR, with assumption of a Gaussian distribution. In this work, however, we only use the mean cost which represents R. We aim to directly predict CVaR from the distribution of costs that arise through this analysis and the environment itself.

C. Transforming Pointclouds to Costmaps

We aim to construct a network whose inputs are the LiDAR pointcloud, and whose output is a CVaR costmap (2-D). There are many different approaches for processing LiDAR data using neural networks, but engineering a custom pointcloud-to-image neural network architecture is beyond the scope of this work. For simplicity, we restrict ourselves to using an image-to-image autoencoder type of network (namely, U-Net with partial convolutions [19]), leaving the difficult task of transforming a LiDAR pointcloud to images for future work. To adapt our pipeline for this approach, we bin pointcloud data into 5 height bins relative to the computed elevation map. We count the number of points which fall into these bins and generate an image from these counts. We end up with input features where each feature channel is a 2-D image (see Figure 6). Image sizes are 400×400 pixels, with each pixel representing 0.1m, for a total area of 40m × 40m covered.

The presence of gaps and occlusions in LiDAR data lead to known and unknown regions in the image-translated input data. These unknown regions need to be handled by the network. A naive approach would be to use zero-padding, i.e. replacing unknown pixels with 0. However, this can lead to blurring and other spurious artifacts at the edges. For this reason, the use of partial convolutional neural networks lends itself particularly well to this image-to-image learning task with unstructured, unknown masks [19].

Additionally, we provide the desired α level as an image to the input of the network. Since α is an image it can vary spatially. The loss function takes this desired α into account per-pixel, and trains the network to produce the correct risk level according to the desired input pattern. During training, we use a uniformly distributed smoothed random pattern, whose distribution spans α ∈ [0,1].

Figure 8 outlines the costmap network architecture used in our approach. The pipeline consists of three components: 1) Pointcloud-to-image features transform, 2) an image-to-costmap translation network using a PartialConv U-Net, and 3) VaR (V_θ(m,x,α)) and CVaR $\left({\widehat{C}}_{\theta }(m,x,\alpha )\right)$ outputs, which are fed into the loss function (Equation 13). The loss is averaged pixel-wise across the known mask regions only. This implies that each α value is independent from its neighbors.

During both training and inference time, we expect to use only a certain distribution of patterns of α. A smoothly varying α pattern may emphasize correlations between neighboring pixels, while a completely random α image might encourage less correlations (See Figure 7).

Figure 9 shows corresponding outputs for the inputs shown in Figure 6. The leftmost column shows the ground truth traversability cost, while the next 3 columns show CVaR at varying α levels (α = 0.1,0.5,0.9). Note that CVaR steadily increases as α increases, and generally is a greater value than the traversability cost. The next three columns show CVaR at the same varying alpha levels, minus the VaR output of the network when α = 0.1. This provides more insight into the changes in VaR and CVaR as α is increased. The final column shows the same CVaR − VaR_0.1 except with a varying α input, in a radially decaying pattern, with high α = 1 in the center, and low α=0 at the edges. This kind of pattern could be useful for a risk-aware system which wishes to be more conservative when planning motions in the regions near the robot. Such a behavior could be useful when the user believes he can rely more on data closer to the robot with greater sensor coverage, as well as wishing to allow for a relaxed commitment to avoiding traversability risks further away.

D. Training

Next we describe some details of training and hyperparameters. We used weights for the CVaR loss (13) of λ_V = 10.0, λ_C = 1.0 λ_m = 1.0e−4. We used a Huber smoothing coefficient of h=1.0e−3. The network was trained on a 90 : 5 : 5 split of training, validation, and test data. We used Adam optimization with an initial learning rate of 0.0005, and batch size of 1. No pre-training of the network was used, weights were initialized with Xavier random weights. During training, data augmentation was used - including rotation, translation, scaling, shearing, and flipping. Training took about 16 hours on a 12GB NVIDIA Tesla K80 GPU.

A. Evaluation Metrics

We introduce three evaluation metrics for assessing the quality of VaR and CVaR learning for a given value of α. First, we compute the implied-α (I_α) value, which is a measure of how closely the predicted VaR value matches the true quantile:

An accurate model should have a value of I_α ≈ α. Note that we sum over all valid (non-masked) pixels and data samples. The denominator counts the number of non-masked pixels.

The R² coefficient of determination metric is useful for comparing the explanatory power of a model relative to the distribution of the data. To quantify the modeling capacity of the network when compared to other baselines, we use a pseudo-R², which is a function of α. For assessing pseudo-R² for VaR, we use the following metric [16]:

where ${\overline{V}}_{\alpha }$ is the constant α-quantile computed from the training data. This metric compares the Koenker-Bassett error of a quantile regression model against the total absolute deviation of the data from an independent α quantile. A value of 1 is the best, reflecting total explanation of the dependent variable, while a value less than 0 is poor, implying worse explanatory power than the simple quantile.

Finally, to assess the CVaR modeling, we construct a similar pseudo-R² metric for CVaR. Instead of the Koenker-Bassett error, we use the empirical CVaR error (see [24] for a rigorous discussion of a similar but computationally less tractable approach). Our CVaR pseudo-R² metric compares the total absolute error between our CVaR model and an average empirical CVaR computed from the training data:

where ${\overline{C}}_{\alpha }$ is the constant α-CVaR computed from the training data.

B. In-distribution (ID) Performance

We begin by evaluating the in-distribution (ID) performance of the network. Figure 12 shows a boxplot of the three evaluation metrics described above, testing on the held-out test data, for a network trained on all 6 datasets. The box plots show the distribution of metrics computed per-sample. Implied α holds up well, forming tracking α, while VaR R² and CVaR R² are close to 1 for most values of α. As α nears 0.9 and 0.95, the CVaR R² drops closer to 0, implying less of the data is being accurately explained by the model. This is likely due to the distribution of traversability costs lying between 0 and 1. When α is high, both VaR and CVaR are more frequently predicted to be 1. This results in a lower CVaR R² since the denominator will be small.

We also plot evaluation metrics on each of the 6 dataset categories individually (Figure 12). This allows us to evaluate the differences between each dataset. We note that the Limstone Cave dataset has the highest levels of risk, which results in more frequent distortion of the CVaR R² metric for high α levels. Nevertheless, all datasets perform well, with pseudo-R² values close to 1.0.

C. Out-of-Distribution (OOD) Performance

To evaluate out-of-distribution (OOD) performance, we train the network on all data except the one dataset, and then evaluate the network on the OOD dataset (which the network has not seen during training). We do this by holding out the Limestone Cave dataset (Figure 13). The Limestone Cave dataset has the most different and challenging terrain with the highest average risks, and with many features not present in the other datasets, such as sharp jagged rocks, narrow passages, channels of water, and rough floors. We see that OOD performance on this dissimilar dataset desires improvement, in contrast to OOD performance on other more similar datasets (not shown). However, the Limestone Cave OOD model is still able to maintain healthy pseudo-R² statistics (>0) for a large range of α.

D. Comparisons against baselines

Finally, we compare our approach against three baselines: 1) the handcrafted traversability risk model which relies on assumptions of Gaussianity and accurate mean and variance, described in Section III-B, 2) a model trained to predict the traversability cost using an L1 loss function only, and 3) a model trained to predict both traversability mean and variance using a Gaussian negative log likelihood (NLL) loss [12], which is then used to compute VaR and CVaR assuming a Gaussian distribution. We computed metrics by taking the average over all test datasets, shown in Figure 14. The L1 Loss model does not capture any quantile information and does not depend on α. Therefore, it does not capture the VaR or CVaR as well as the trained model. The hand-crafted traversability cost model also does not perform well - it fails to accurately capture the quantile (VaR) for most values of α. It also does not provide as accurate CVaR values as the learned model. The NLL loss model fairs relatively well predicting CVaR, but less-so for VaR. However, the learned CVaR model still outperforms it in the CVaR statistic.

We also consider the computation time of using the handcrafted traversability cost method vs. our CVaR learning method (Table II). The CVaR learning approach tends to be almost 5x more efficient then the handcrafted approach. (Both methods currently rely on ground segmentation and elevation mapping steps.) We see that replacing the handcrafted costmap with the learned one removes the largest bottleneck in the traversability assessment pipeline.