Hybrid Imitative Planning with Geometric and Predictive Costs in Off-road Environments
Learning
-based methods successfully avoid previously obstacles but can struggle Abstract —Geometric methods for solving open-world off-road navigation tasks, by learning occupancy and metric maps, provide good generalization but can be brittle in outdoor environments that violate their assumptions (e.g., tall grass).
Learning-based methods can directly learn collision-free behavior from raw ob- servations, but are difﬁcult to integrate with standard geometrybased pipelines.
This creates an unfortunate conﬂict – either use learning and lose out on well-understood geometric navigational components, or do not use it, in favor of extensively hand- tuned geometry-based cost maps.
In this work, we reject this dichotomy by designing the learning and non-learning-based components in a way such that they can be effectively combined in a self-supervised manner.
Both components contribute to a planning criterion: the learned component contributes predicted traversability as rewards, while the geometric compo- nent contributes obstacle cost information.
We instantiate and comparatively evaluate our system in both in-distribution and out-of-distribution environments, showing that this approach inherits complementary gains from the learned and geometric components and signiﬁcantly outperforms either of them.
I. I NTRODUCTION
How can we enable a robot to swiftly traverse open-world environments while minimizing heuristic and time-intensive hand-engineering, like those depicted in Fig.
The robot should receive coarse goal direction from a human supervisor, and use this direction, along with its sensor suite and prior experience, to make its own decisions about what actions to take to reach the destination.
A solution to this problem would enable users to direct robots across unfamiliar territory without requiring them to signiﬁcantly change or tune components of the system.
For example, imagine a rescue worker tasking a search-and-rescue robot to quickly search a series of locations in an unmapped dense forest.
A major challenge to developing such a system is enabling it to both draw on prior experience and adapt its behavior to new environments.
While learning is a powerful way to deal with open-world environments, most learning-based methods studied are difﬁcult to integrate with non-learning-based navigation pipelines.
This creates an unfortunate “either-or" dichotomy – either use machine learning and re-design the entire navigation stack around it, or do not use it, in favor of extensively hand-tuned geometry-based cost maps and structured sensors.
The key idea of our approach is to construct an algorithm by designing the learning and non-learning-based components in a way such that they can be a r X i v : . 10948v [ c s . R
O ]
N ov effectively, and easily, combined and created without labeling any data.
Fig.
1 depicts this synthesis.
One classic approach is to perform online geometric mapping and traversability estimation and then use these estimates, along with a hand-tuned cost function, for planning feasible paths [1– 4].
With careful design and incorporation of prior knowledge into the decision-making pipeline, these approaches, which we term “geometry-based” and “geometric costmap”-based, are a standard and performant approach.
Their main drawback, however, is a general inability to automatically tune the costmap parameters with the inclusion of additional experience.
For example, if the costmap assumes all densely-populated points correspond to rigid and untraversable obstacles, traversable tall grass may cause the robot to be too conservative and avoid the grass; if a height threshold is included to compensate for this effect, then small, untraversable obstacles like rocks may cause the robot to be too aggressive and get stuck on the rocks.
Another approach to robot navigation is learningbased, e.g. goal-conditioned imitation learning and policy-based reinforcement learning, yet these are difﬁcult to integrate with real-world robotic systems because they are inscrutable and directly output actions.
Can we build on both lines of work by addressing their drawbacks in a principled way?
The main insight in our work is that, instead of trying to reconcile conﬂicting actions commanded by geometry-based and learning-based components, we can instead utilize both components to contribute terms to a shared navigational cost function.
This cost function represents the hypotheses of both methods about which future trajectories will or will not lead to collisions.
Once a shared cost function combining geometrybased and learning-based reasoning has been produced, a standard planning method can decide on the best path to take informed by this cost.
We summarize this idea, and an intuition for why the geometry-based and learning-based components might provide complementary strengths, in Fig. 2.
The key contribution of our work is a framework to combine learned costs with a geometric costmap into a shared navigational cost function, enabling a system that can leverage the ability of learning-based methods to pick on subtle cues from high-dimensional observations and the reliability and generalization capabilities of geometric methods.
We instantiate and empirically evaluate our system in a variety of in-distribution and novel environments in a high-ﬁdelity simulator.
We ﬁnd that our hybrid approach is able to combine the beneﬁts of its components and out-perform either of them in challenging domains, signiﬁcantly improving performance in out-of-distribution scenes.
II. H YBRID I MITATIVE P LANNING WITH G EOMETRIC AND P REDICTIVE C OSTS
In our problem setting a mobile robot receives highdimensional sensory observations and global localization information, and is tasked with using these sources of information to navigate to a provided goal in an unmapped, off-road environment partly populated with untraversable terrain (e g. large and small obstacles, steep hills).
Let o t
=
( i t , l t , x t ) it adeptly denote the observations that are available to the robot, where x t ∈ R 3 is the robot’s position estimated by odometry, i t ∈ [0 , 1] H
i
×
W
i ×
is an RGB-D image, and l t ∈ R N t ×is a point cloud provided by a LiDAR sensor.
Let d ∈ Rdenote the provided goal.
Let τ ∈ R 2 H denote a trajectory of potential future positions: τ .
= x t +1: t + H , and τ i denote x t
+ i .
Our main idea is to design a system that allows learned models to easily integrate with standard geometric reconstruction and planning pipelines.
Learned costs and represented as a function of trajectories and learned via density estimation to imitate collision-free data, and geometric costs are represented as a function of positions , which can be composed into a function of trajectories, and then combined into a combined single planning criterion.
Finally, destination directives are represented as positions and included as a ﬁnal cost.
We term our method Hybrid Imitative Planning (HIP), and present a system diagram in Fig.
While we expect the capability to integrate these separate components to be generally useful, we have some intuition about how the utility may manifest.
Geometric costmap-based planning may confer safety around large, easily-sensed objects, like boulders and trees, yet it cannot incorporate subtle visual cues, and more generally, cannot be improved from additional robot experience.
Learningbased planning may offer the capability to identify terrain directly estimated as traversable from experience, such as grass and small bushes, yet it may be risky around objects that appear signiﬁcantly different than those in the training data.
In designing these two approaches into a single system, we expect the system to be zero-shot robust to objects that appear signiﬁcantly different from the training data and able to identify subtle aspects of the terrain that afford or inhibit traversability.
In this section, we present an abstract overview of how geometric traversability costs can be integrated with learned models, and then present a speciﬁc instantiation of a navigation system based on this design in Section III.
Designing geometric costmaps.
We assume the geometry- based component is available as a costmap : C t ( · ) : R 2 (cid:55)→ R .
This decision enables interpretable design of geometric heuristics g ( l t ) =
C t that process the LiDAR into a function of positions.
Let us denote the composition of the costmap generation and evaluation of it at a position x t as C costmap ( x t ; o ≤ t ) .
= g ( l t )( x t ) .
We defer discussion of how g is implemented to Section III.
Designing learned costs.
In order to combine geometric and learned costs, we must construct a learner that processes the high-dimensional sensor data to assigns costs to trajectories.
Our learning-based costs aim to predict whether a given trajectory would be collision-free – whether it corresponds to a traversable path.
Formally, we construct the learning-based component as a conditional probability density function of trajectories τ : q ( τ | o ≤ t ) : R 2 H (cid:55)→ R , and learn q from data that excludes collisions.
By satisfying this assumption and training q via a maximum likelihood objective, q should assign high values to a subset of the traversable trajectories and low values to this subset’s complement, which will include untraversable trajectories.
Given q , we can construct costs as C learned ( τ ; o ≤ t ) .
= − log q ( τ | o ≤ t ) .
Note that because q is a function of trajectories, not positions, it has a higher modeling capacity than if it were parameterized by a costmap.
We defer further discussion of how we learn q to Section III.
Designing directive costs.
In order to direct the robot to the ﬁnal desired destination ( d ), we incorporate a cost that encourages forward progress towards the ﬁnal destination.
Denote this cost C directive ( τ ; o ≤ t , d ) .
We defer discussion of how we construct C directive to Section III.
Combining geometric costmaps and learned costs.
Given our goal of integrating learning-based and geometry-based planning, we design π to use a receding-horizon state-space planner as shown in Eq.
[Equation]
Given a planned trajectory, τ , we use it to compute a t as a simple position-tracking PID controller, written as a t = f ( τ ) .
III. I NSTANTIATING THE S YSTEM
Implementing learned costs.
As previously discussed, our learned costs are designed to be a conditional probability density function of future trajectories, q ( τ | o ≤ t ) ﬁt to data that excludes collisions.
We implement q as a “Deep Imitative Model” [5].
We adapted the open-source PyTorch implementation of imitative models released in Filos et al.
[6] and adapted the input to use RGB-D. This model uses a MobileNetV encoder [7].
During training, we follow the method of Rhinehart et al.
[8] to induce an upper-bound, η , on q by perturbing the training trajectories with a Gaussian.
We defer further datadependent implementation details, including a full table of hyperparameters, to Section V.
Implementing geometric costmaps.
We use a LiDAR sensor to produce a local point cloud, from which a terrain traversal cost is computed.
We use the “costmap_2d” implementation provided by ROS to compute a discrete 2D costmap for the ground plane, denote C (cid:48) costmap [9].
We apply a nonlinear transformation to C (cid:48) costmap so that its maximum value corresponds to η / H .
This ensures (cid:80) Hi {+=
+} C costmap ( τ i ) ≤ max τ q ( τ | o ≤ t ) , which makes tuning the φ parameter simpler, as the costs are roughly in the same range.
This transformation is given by
[Equation]
Implementing directive costs.
Similar to the region goals described by Rhinehart et al.
[5], we designed a directive cost that penalizes τ with cost δ if it does not end within a particular region a short distance away from the robot, in the direction of d .
We construct this region as a rectangle with width 2m , center axis in the direction of d , offset towards d by 3m .
Fast planning.
In order to quickly solve the optimization problem in Eq.
(1), we employ a trajectory library with size with K = 200 .
The future trajectories (described in Section V, were clustered using k-means, and the centroid, ¯ τ k , of each cluster was appended to a library, K = { ¯ τ k } Kk =1 .
During inference, we perform approximation optimization of Eq.
(1) using argmin τ ∈K L ( τ ; o ≤ t , d ) .
System and environment overview.
We instantiate our system on a Clearpath Husky UGV deployed in a photorealistic outdoor navigation simulator.
The default sensor suite on the Husky includes a 6-DoF IMU, a GPS unit for approximate global position estimates and wheel encoders to estimate local odometry.
We added a forward-facing wide ﬁeld-of-view RGBD camera and a LiDAR sensor.
During data collection and evaluation, we heuristically detect collisions using IMU and odometry data.
The simulator consists of a Unity backend and is tightly integrated with the ROS stack on the robot.
The environment consists of an obstacle-rich, enclosed geofence where dense rigid trees, bushes, and traversable tall grass are scattered throughout.
While the geometric costmap may correctly identify trees as hazardous, it can fail to disambiguate traversable grass and untraversable bushes.
Furthermore, it does not reason about other terrain properties that may lead to collision, such as the terrain slope.
In order for the robot to succeed, it must carefully navigate through dense obstacles over traversable terrain.
IV. R ELATED W ORK
Geometry-based goal-directed navigation.
Existing methods for traversability estimation include geometry-based, appearance-based, and proprioceptive systems, as categorized by Papadakis [4].
Geometry-based methods build a 2.5D or 3D terrain map that is used to extract features, such as the maximum, minimum, and variance of the height and slope of the terrain [10, 11].
Planning algorithms for such methods can take into account the stability of the robot on the terrain [2, 12].
Since sensor and localization uncertainty can play a large role in the construction of environment maps, various methods exist for estimating the probability distributions of states based on the kinematic model of the vehicle and the terrain height uncertainty [3, 13].
For example, Fan et al.
[1] construct distributions of traversability costs for risk-aware planning.
These approaches rely on hand-crafted models to determine risks to the robot when traversing over various terrains.
However, these models often rely on simplifying assumptions, and may not consider the compressibility or compliance of geometric features, especially with respect to vegetation [14].
In contrast, our hybrid approach allows us to leverage geometry-based goal-directed navigation in tandem with a learned model, which enables our approach to overcome misspeciﬁcations of the geometry-based heuristics.
Learning-based goal-directed navigation.
A variety of learning-based methods have studied the acquisition of goaldirected behavior, broadly classed as imitation learning (IL) [13, 15–18] and reinforcement learning (RL) [19–21].
Goal-conditioned IL typically requires goals to be speciﬁed during training and do not extend well beyond demonstrations.
A drawback to these more general IL and RL systems is that they are difﬁcult to interpret and incorporate into existing geometry-based goal-directed navigation pipelines.
Planning and navigation in unstructured environments can be greatly improved by learning environment traversability using prior experience, but previous approaches to explicitly representing environment traversability require expensive human supervision or traversability heuristics [22–25].
Recent progress in using topological graphs to implicitly reason about traversability [18, 26–28] gives a promising way to learn from prior experience but has not been demonstrated for long-range navigation in complex, unstructured environments.
In contrast, our hybrid approach employs a form of self-supervised learningbased explicit trajectory traversability estimation in tandem with geometry-based positional traversability estimation, which enables our approach to more robustly deal with previouslyunseen complex obstacles and terrains.
V. E XPERIMENTS
We designed our experiments to answer the following questions: Question 1: Can our combined approach achieve collision-avoidance and navigation performance superior to its constituent components?
Question 2: How does varying input modality and component weighting affect HIP?
To answer these questions, we measure the rate of navigational success for randomized start-goal pairs in two
[Figure] different areas (in-distribution) in the simulator (which was described in Section III).
This success rate metric quantiﬁes the performance of the model when globally directed to a determined goal; speciﬁcally, how often it succeeds in reaching the goal unharmed.
We report a normalized version of this metric relative to the performance of a planner operating with a coarse-grained global obstacle map, which achieved a success rate of 0 . 72 .
This global planner failed sometimes due to leading the robot onto hills on which it capsized and into inescapable dead-end areas with small obstacles.
This “oracle” planner violates the problem assumption of deployment to an unmapped environment, as it has access to a map.
Towards Question 1 , we expect the geometric costmap method to struggle with smaller untraversable obstacles, thus affecting its capability to efﬁciently navigate given a global direction.
We expect the learner, on the other hand, to be able to generate sequences of movement through areas that are perceptually similar to traversable scenes in the training data.
Since any learned model is susceptible to errors under distributional shift, we might expect the learner to sometimes fail to produce collision-free trajectories when it encounters obstacles that are visually very different from those seen in the training data.
Finally, we expect our proposed hybrid approach to harness the strengths of its components to outperform them with minimal hand-engineering effort.
We expect the improvement to result primarily from superior obstacle-avoidance behavior.
Towards Question 2 , we ﬁrst investigate the effect of φ , which controls the importance of the component costs on the planning criterion (Eq.
By varying φ to identify the optimal value, φ ∗ , we determine if they complement each other ( φ ∗ ∈ (0 , 1)) , or whether one dominates ( φ ∗ ∈ { 0 , 1 } ) .
Furthermore, through the ablation, we can understand the importance of each sensor; we expect the learner’s performance to be maximized when all sensor modalities are included.
Baselines.
Beyond the components of our method, we include three other baselines to further contextualize our system’s performance.
Behavior Cloning (BC) baseline:
We trained a goal-conditioned BC baseline using the same data the learner used, as well as a nearly identical neural-network architecture to that of the learner.
The BC baseline is trained to directly predict a control given a provided goal, rather than a probability density function over trajectories.
Straight baseline:
In order to contextualize the importance of reactivity to perceptual cues, we include a baseline that plans an action to track the straight-line segment from the robot to the goal.
This baseline does not process the perceptual data, and therefore cannot react to obstacles.
Random baseline: Finally, we include a baseline that uniformly samples a random action from the robot’s discretized action space, which further contextualizes the difﬁculty of the problem.
Data collection and model learning.
We performed the following steps to automatically collect training data in the simulator.
First, we created a geofence and a generated a set of random starting poses within it.
Next, we randomized the robot’s starting pose among this set and drove it with “sticky” (executed for multiple frames) random actions.
As per our method’s data requirements described in Section II, we automatically removed sequences that resulted in collisions.
Although the explicit collision data could be employed to reﬁne the model, we found this method to be effective even when the collision data was discarded entirely.
Collisions were heuristically identiﬁed in three different ways, and the robot was randomly respawned after any collision.
The ﬁrst heuristic is a stuck collision , which triggers when the robot is stationary for over 4 seconds.
The second is a trapped collision , which triggers when the robot does not move over 3m over a 10s period, which prevents tight circular movement or very slow motion.
The ﬁnal heuristic is a capsized collision , which uses the IMU to identify when the robot is capsized.
As part of each recorded positional trajectory, RGB-D and odometry data was recorded.
To create the set of examples for training, the data was postprocessed by subsampling at 5Hz.
Each example uses 10 past timesteps and a one RGB-D visual input, and is trained to predict H = 10 timesteps of future positions via a maximum likelihood objective.
Over > 200 k examples were collected, with which our Learner was trained to maximize the likelihood of the ground truth odometry positions conditioned on the past odometry and visual information: max θ E ( o ≤ t ,τ ) log q θ ( τ | o ≤ t ) . [Table]
[Table] Table II summarizes the hyperparameters of our method.
Experimental setup.
The simulator we employed in our experiments was described in Section III.
To measure success rate, s goalrate , the robot was randomly spawned 300 times in the geofence with a goal at least 10m away.
A constant seed was utilized for generating start and goal points across each method.
We conducted this experiment in two environments: (1) the environment from which data was collected, and (2) an out-of-distribution environment containing novel obstacles.
Results analysis.
Our primary results are shown in Fig.
The naïve baselines – Random and Straight – illustrate the difﬁculty of the problem.
We ﬁnd our method to be the most performant.
The Learner signiﬁcantly outperforms the Behavioral Cloning baseline, as it represents multiple possible futures and defers goal-conditioning until test time, when the planner can select the most appropriate trajectory for the given goal.
The Geometric Costmap outperforms the naïve baselines, and performs similarly to BC.
It cannot independently model traversability for rigid objects shorter than grass, such as bushes or rocks, but is adept at identifying areas with larger, more distinguishable untraversable objects.
Table I contains the result of the ablation analysis.
The RGB-D Learner outperformed both RGB and Depth alone, thus illustrating the signiﬁcance of both modalities in determining traversability.
This gap is further widened in out-of-distribution environments, where the learned costmap suffers due to unreliable predictions.
In Fig.
the results of the φ hyperparameter sweep are presented.
These results afﬁrm the efﬁcacy of both components of our method, show that multiple φ values are performant.
In Fig.
5, we show example rollouts from our method navigating dense obstacles; we observed that it is capable of winding through complex areas with many obstacles if an open path exists.
In Fig.
6, a set of qualitative examples for our method, the Learner, and the Costmap are depicted.
In these examples, we often observe the Costmap to identify a subset of the impassable obstacles, while the Learner reﬁnes the remaining viable paths to account for previous odometer positions and objects perceived in the RGB-D image (which may be undetected by LiDAR).
This results in the most effective navigation around visually perceivable obstacles.
VI. D ISCUSSION
We proposed HIP, a method designed to ﬂexibly incorporate learning-based and geometry-based components into a single cost function for goal-directed navigation in open-world off-road environments.
We evaluate HIP in a high-ﬁdelity simulator, and ﬁnd that HIP shows signiﬁcant improvement over both of its constituent components, as well as baselines.
Ablations of sensory inputs conﬁrm the efﬁcacy of both RGB and depth data to the system.
A hyperparameter sweep of the primary parameter illustrates the existence of multiple performant values.
The effectiveness of our approach illustrates that both learning-based and geometry-based components for autonomous navigation can be integrated effectively if they contribute distinct but complementary terms to a shared cost function.
That is, instead of sharing control between different types of methods, we simply add their contributions to a cost function used by a standard model-predictive control method.
This approach suggests promising directions for future work, integrating other types of learned models, as well as additional sensory modalities.
Furthermore, since our learned costs can be integrated with arbitrary goal representations into a standard planner, a promising direction is to further study other types of objectives that can be accomplished with our method