Probabilistic Traversability Model for Risk-Aware Motion Planning in Off-Road Environments

Traversability analysis is a key component of off-road navigation algorithms; a more complete summary of various approaches is provided in [17], including representations based on proprioceptive measurements [19, 20], geometric features [21, 22, 23, 1] and combination of geometric and semantic features [24, 25, 10]. WayFast [16] is a more recent approach, similar to this work, that proposes to represent traversability by learning traction coefficients for a unicycle model from terrain perception data. Another recent work [2] represents traversability as the probability of a quadruped robot to stabilize itself on uneven terrain, based on 3D occupancy data of the terrain. In [26], speed and gait policies are learned based on terrain semantics and human demonstrations; these policies provide a novel interpretation of the terrain’s traversability and can be used by the robot’s motor control policy. A key limitation, however, is that these point estimates of traversability do not capture the uncertainty of terrain properties on similar-looking terrains. Instead, our approach represents traversability as the distribution of traction parameters in the dynamics model.

After learning the traction distribution in the dynamics model, a further challenge exists in incorporating this model into the planner. Despite capturing various types of uncertainties and risks, many existing methods still plan with the nominal or expected parameters in the dynamics model, such as [25, 27, 17]. Alternatively, our work is inspired by [28] that proposes a general framework for optimizing the conditional value at risk (CVaR) of the objective under uncertain dynamics, parameters and initial conditions, by taking extra samples from sources of uncertainty. In this work, we evaluate each control sequence based on traction samples and use the CVaR of the noisy realizations of the objectives instead of the nominal objective. Additionally, we propose a more computationally efficient approach which computes the nominal objective using trajectory rollouts based on the CVaR of traction parameters. Compared to WayFast [16] that uses the expected traction values (a risk-neutral special case of our second method), our approach can produce behaviors that are more risk-averse by adjusting the worst-case quantiles used to compute the CVaR of traction.

Data collection in practice is often expensive and limited in diversity, so it is important to know when a learned model cannot be trusted (e.g., due to input features that are significantly different from training data). NN uncertainty estimation is well studied in the machine learning literature (e.g., see survey [29]), where representative techniques include Monte Carlo dropouts [30], ensembles [31], and single-pass methods [32]. Most similar to the single-pass technique [32] that only requires a single neural network evaluation to estimate uncertainty, our work tries to capture the aleatoric uncertainty (the inherent process uncertainty that cannot be reduced with more data) by predicting a distribution, and the epistemic uncertainty (the model uncertainty that can be reduced with more data) by leveraging the latent space density of NN. Compared to using dropouts or ensembles, single-pass methods are attractive, because they do not require taking extra samples or using more memory.

Consider the discrete time system:

where $\mathbf{x}_{t}\in\mathbf{X}\subseteq\mathbb{R}^{n}$ is the state vector, $\mathbf{u}_{t}\in\mathbb{R}^{m}$ is the control input, and $\bm{\psi}_{t}\in\bm{\Psi}\subseteq\mathbb{R}^{q}$ is the parameter vector that captures the terrain traction. For each mission, terrain-dependent parameter vectors are sampled from the ground truth distribution $\bm{\psi}\sim p^{*}(\cdot|\mathbf{o}_{\mathbf{x}})$ for every $\mathbf{x}\in\mathbf{X}$ and the associated terrain features $\mathbf{o}_{\mathbf{x}}\in\bm{O}$. For concreteness, we use the following unicycle model

where $\mathbf{x}_{t}=[p_{t}^{x},p_{t}^{y},\theta_{t}]^{\top}$ contains the X, Y positions and yaw, $\mathbf{u}_{t}=[v_{t},\omega_{t}]^{\top}$ contains the linear and angular velocities, $\bm{\psi}_{t}=[\psi_{1},\psi_{2}]^{\top}$ contains the linear and angular traction values $0\leq\psi_{1},\psi_{2}\leq 1$, and $\Delta>0$ is the time interval. Intuitively, traction captures how much of the commanded velocities can be achieved and is a good indicator for terrain traversability for fast off-road navigation.

As this work focuses on achieving fast navigation to a given goal position, we adopt and modify the minimum-time formulation used in [17], but any other task-specific objectives can be used instead. Given initial state $\mathbf{x}_{0}$ and goal position $\mathbf{p}^{\text{goal}}\in\mathbb{R}^{2}$, the problem of finding a sequence of control $\mathbf{u}_{0:T-1}$ can be written as

where $\phi(\mathbf{x}_{T})$ and $q(\mathbf{x}_{t})$ are the terminal cost and the stage cost:

where $s^{\text{default}}$ is the default speed for estimating time-to-go at the end of the rollout, $\Delta$ is the sampling duration, $\mathbf{p}_{t}$ is the robot position at $t$, and $w^{\text{dist}}>0$ is the weight for penalizing distance from the goal. To avoid accumulating costs after robot reaches the goal, we use an indicator function $\mathbb{1}^{\text{done}}(\mathbf{x}_{0:t})$ that returns $1$ when any state $\mathbf{x}_{\tau}$ has reached $\mathbf{p}^{\text{goal}}$ for $0\leq\tau\leq T$, and returns $0$ otherwise. Intuitively, the objective encourages the robot to arrive at the goal location as quickly as possible.

While the problem (3) can be optimized via non-linear optimization techniques such as Model Predictive Path Integral control (MPPI [33, Algorithm 2]), the main challenge is that the terrain traction $\bm{\psi}$ is uncertain. To address this issue, existing techniques try to learn the expected traction, or use nominal traction while penalizing undesirable terrains manually. However, these approaches are either risk-neutral or require human expertise in cost design. In this work, we propose to learn the full traction distribution empirically (Sec. IV) that can be used to design risk-aware costs with easy-to-tune risk tolerance (Sec. V).

Given the set of system parameters $\bm{\Psi}$ and the set of terrain features $\bm{O}$, we want to model the conditional distribution

where $p_{\bm{\theta}}$ is a probability distribution parameterized by $\bm{\theta}$, which in practice can be learned by a NN using empirically collected dataset $\{(\mathbf{o},\bm{\psi})_{k}\}_{k=1}^{K}$ where $K>0$. A real world example can be found in Fig. 2 where a Clearpath Husky was manually driven in a forest to build an environment model and collect traction data. Note that the semantic and geometric information about the environment can be built by using a semantic octomap [34] that temporally fuses semantic point clouds. We used PointRend [35] trained on RUGD off-road navigation dataset [11] to segment RGB images and subsequently projected the semantics onto lidar point clouds. To estimate the true linear and angular velocities of the robot, we could not rely on the wheel-encoders due to wheel slips. Therefore, we used direct lidar odometry [36] that produced accurate pose estimates even when driving through tall grass, and the resultant pose estimates were fused with IMU measurements in an extended Kalman filter to obtain the high-rate velocity estimates. The velocity estimates were further filtered to reduce noise due to bumpy terrains. Finally, the traction values were computed as the ratios between the estimated and the commanded velocities and stored for offline training.

Given terrain features, traversed path and estimated traction values, we can train a NN (e.g., with convolutional layers followed by fully connected layers) to map local terrain features to categorical distributions over discretized traction values in order to capture rich terrain properties. To facilitate planning, we store learned traction distributions in a map $\mathbf{M}_{\bm{\theta}}$, where each cell $\mathbf{M}_{\bm{\theta}}^{h,w}$ indexed by row $h$ and height $w$ stores the traction distribution $p_{\bm{\theta}}^{h,w}:\bm{\Psi}\rightarrow\mathbb{R}$ that has already been conditioned on the associated terrain features.

As the learned traction predictor is trained on limited data, its predictions based on terrain features significantly different from training data are unreliable, which can lead to degraded navigation performance. Therefore, we fit a density estimator in the latent space of the trained NN in order to use the predicted likelihood as a measure of model uncertainty.

We first apply principal component analysis (PCA) to the latent space features that correspond to all the terrain features observed during training $\bm{O}^{\text{train}}=\{\mathbf{o}_{k}\}_{k=1}^{K}$. Next, we fit a Gaussian Mixture Model (GMM) for the entire training dataset in the reduced latent space, where the likelihood of observing a particular feature $\mathbf{o}_{k}$ is denoted as $p_{\bm{\theta}}^{\text{latent}}(\mathbf{o}_{k})$. We design a simple confidence score $g$ based on the log-likelihood of the query data normalized between the maximum and minimum log-likelihood observed in training data:

Note that $g(\mathbf{o})$ is not limited to $[0,1]$ and lower values indicate that the terrain features are less similar to the training data. With the NN trained in the environment shown in Fig. 2, we project the latent space features to the first 2 principle components and fit a GMM with 2 clusters. During deployment, terrain features with confidence score below some threshold $g^{\text{thres}}\in[0,1]$ are deemed out-of-distribution (OOD) and the OOD terrains should be explicitly avoided during planning via auxiliary penalties. This strategy improves navigation success rate when the NN is deployed in an environment unseen during training (see Sec. VI-C).

We adopt the Conditional Value at Risk (CVaR) as a risk metric because it satisfies a group of axioms important for rational risk assessment [37], but the conventional definition assumes the worst-case occurs at the right tail of the distribution. In this work, we define CVaR at both the right and left tails (see Fig. 5) at level $\alpha\in(0,1]$ for the random variable $Z$ and its possible realization $z\in\mathbb{R}$ as follows:

where the right and left Values at Risk are defined as:

Intuitively, ${\text{CVaR}_{\alpha}^{\rightarrow}(Z)}$ and ${\text{CVaR}_{\alpha}^{\leftarrow}(Z)}$ capture the expected outcomes that fall in the right tail and left tail of the distribution, respectively, where each tail occupies $\alpha$ portion of the total probability. Notice that either definition of CVaR produces the mean when $\alpha=1$.

In order to account for the risk of obtaining high cost due to the uncertain system parameters $\bm{\psi}_{t}$, we propose to optimize two modified versions of the cost function (3). Fig. 5 gives a high-level illustration of the core ideas behind the two cost functions.

In order to solve the optimization problem (3), we adopt MPPI [33, Algorithm 2] to generate control for achieving fast navigation to goal. This approach is attractive because it is derivative-free, parallelizable on GPU, and works with our proposed risk-aware cost formulations (15) and (17). The MPPI planners run in a receding horizon fashion with 100 time steps and each step is 0.1 s. The max linear and angular speeds are capped at 3 m/s and $\pi$ rad/s, and the noise standard deviations for the control signals are 2 m/s and 2 rad/s. The number of control rollouts is 1024 and the number of sampled traction maps is 1024 (only applicable for the CVaR-Cost). We use probability mass functions with 20 uniform bins to approximate the parameter distribution. The CVaR-Cost planner is the most expensive to compute, but it is able to re-plan at 15 Hz while sampling new control actions and maps with dimension of $200\times 200$. Planners that do not sample traction maps can be executed at over 50 Hz. A computer with Intel Core i9 CPU and Nvidia GeForce RTX 3070 GPU is used for the simulations, where majority of the computation happens on the GPU.

We consider a grid world scenario where “dirt” and “vegetation” cells have known traction distributions, as shown in Fig. 7. Vegetation patches are randomly spawned with increasing probabilities at the center of the arena, and a robot may experience significant slow-down for certain vegetation cells due to vegetation’s bi-modal distribution. The mission is deemed successful if the goal is reached within 15 s.

Overall, we sample $40$ different semantic maps and $5$ random realizations of traction parameters for every semantic map. The traction parameters are drawn before starting each trial and remain fixed. The benchmark results can be found in Fig. 6, where we compare the two proposed costs with existing methods, namely WayFAST [16] that uses the expected traction and the technique in [17] that assumes nominal dynamics while adjusting the time cost with the CVaR of linear traction. The takeaway is that the proposed methods outperform the two existing ones by accounting for the worst-case expected cost and traction. Notably, although the CVaR-Dyn planner does not sample from the entire parameter distribution, it achieves similar performance to the CVaR-Cost planner when $\alpha$ is set sufficiently low. The poor performance of the CVaR-Cost planner can be attributed to the difficulty of estimating CVaR from samples in general when $\alpha$ is low. However, the CVaR-Dyn planner makes conservative assumption about the dynamics, so it does not out-perform CVaR-Cost in achieving low time-to-goal.

To compare our cost formulations against the nominal objective with auxiliary stage costs that penalize states in vegetation cells, we focus on the most challenging setting with 70% vegetation where it is easy to get stuck in local minima. The benchmark result is shown in Fig. 8 where we compare the trade-offs between success rate and time-to-goal achieved by different methods. Although there exists risk tolerance $\alpha$ that allows CVaR-Cost and CVaR-Dyn to obtain better trade-off than having vegetation penalties, their conservativeness in considering the CVaR of cost and traction prevents them from achieving solutions with higher success rate. Therefore, when domain knowledge is available, adding auxiliary costs can help achieve much higher success rate desirable in practice, but tuning the cost may be challenging when a large variety of terrains exist.

In this section, we demonstrate the benefit of the proposed density-based confidence score (8) for detecting terrains that lead to unreliable NN predictions. Because benchmarking different planning algorithms is not the main focus of this section, we use the proposed CVaR-Dyn planner that has been shown to achieve higher success rates than CVaR-Cost and set a low risk tolerance $\alpha=0.2$ (experience has shown that similar results occur using other planners). In order to simulate training and testing environments, we leverage the data collected in two distinct forests, where the first one (visualized in Fig. 2) is used to train the traction predictor, and the second one (whose semantic top-down view is shown in Fig. 10) is used to simulate the test environment. The traction values will be drawn from the test environment’s empirical traction distribution learned by a separate NN as the proxy ground truth. Two specific start-goal pairs are selected in order to highlight the most challenging parts of the test environment with novel features. Each start-goal pair is repeated 10 times for each selected confidence threshold $g^{\text{thres}}$. We investigate two different approaches to prevent the planner from entering OOD terrains with low confidence: (1) assigning zero traction to OOD terrains, and (2) adding large penalties for states entering OOD terrains. Note that the mission is successful if each goal is reached within 30 s.

As shown in Fig. 10, the navigation success rate improves by up to $30\%$ as $g^{\text{thres}}$ increases, because the robot avoids regions where the network’s prediction may be significantly different from the ground truth. Interestingly, using CVaR-Dyn with additional penalties for states entering OOD terrains leads to better time-to-goal while retaining similar success rate. Intuitively, the auxiliary costs make it easier for the CVaR-Dyn planner to find trajectories that move around the OOD terrains. Therefore, it is advantageous to combine the proposed cost formulations with auxiliary costs when domain knowledge is available in order to achieve both high success rate and fast navigation in practice.