Rough Terrain Navigation Using Divergence Constrained Model Based Reinforcement Learning
Anonymous Author(s
Abstract: Autonomous navigation of wheeled robots in rough terrain environments has been a long standing challenge.
In these environments, predicting therobot’s trajectory is challenging due to the complexity of terrain interactions andthe divergent dynamics that cause model uncertainty to compound.
This inhibitsthe robot’s long horizon decision making capabilities and often lead to shortsighted navigation strategies.
We propose a model-based reinforcement learningalgorithm for rough terrain traversal that trains a probabilistic dynamics model toconsider the propagating effects of uncertainty.
Our method increases predictionaccuracy and precision by using a tracking controller and by using constrainedoptimization to ﬁnd trajectories with low divergence.
Using this method, wheeledrobots can ﬁnd non-myopic control strategies to reach destinations with higherprobability of success.
We show results on simulated and real world robots navigating through rough terrain environments.
Introduction
Autonomous wheeled robots are useful for a variety of applications such as environmental monitoring, package delivery, and warehouse operation.
Although these robots are capable of autonomouslynavigating through ﬂat environments, rough terrain still poses a challenge as it requires non-myopicdecision making to avoid getting trapped.
This is challenging as the interactions between the robotand the unstructured terrain are complex and difﬁcult to model [1].
Furthermore, the highly divergent dynamics can cause modeling errors to compound and propagate poorly along trajectories.
Methods that rely on accurate handcrafted dynamics models for decision making, e g. [2, 3, 4],often perform poorly when predictions contradict reality.
Some methods aim to combat this issueby formulating control policies that are robust against model uncertainty [5, 6, 7, 8].
Others abstractthe robot’s dynamics by using traversability maps to provide heuristics for how difﬁcult differentregions are to traverse [9, 10, 11, 12, 13, 14].
However, these approaches are limited to coarseapproximations of traversability and don’t consider dynamic interactions.
Alternatively, reinforcement learning can be used for rough terrain traversal.
Some methods usemodel-free reinforcement learning (MFRL) [15, 16, 17], where control policies are directly optimized.
Others use model-based reinforcement learning (MBRL) [18], where predictive dynamicsmodels are learned and then used for decision making.
Due to their higher sample efﬁciency, MBRLmethods are generally more practical for real world systems where training data is limited and expensive to collect.
However, MBRL methods often have poorer performance as their decision makingprocesses may exploit model inaccuracies.
Previous MBRL algorithms have addressed this issueby considering prediction uncertainty during decision making [19, 20], though characterizing prediction uncertainty for rough terrain navigation is challenging since the divergent dynamics causemodel uncertainty to propagate poorly over long prediction horizons.
We propose a MBRL algorithm for rough terrain navigation, summarized in Sec. 3 and Fig.
Ouralgorithm’s unique method of handling uncertainty allows the robot to more accurately optimize longer trajectories.
Like previous methods [ our performs trajectory optimizationusing learned probabilistic dynamics models to predict trajectories and characterize prediction uncertainty.
However, we train the probabilistic dynamics model using a multistep loss that considershow model uncertainty propagates along trajectories (Sec. 4). We perform closed-loop trajectoryprediction, where the capability of the robot’s trajectory tracking controller is considered duringtrajectory prediction (Sec. 5). The use of a multi-step loss and closed-loop trajectory predictiondecreases long horizon trajectory prediction uncertainty and allows the robot to be more selectiveduring optimization. Our algorithm uses constrained optimization to avoid trajectories that resultin high divergence (Sec. 6), similar to Convergent Planning [8].
Through simulation and hardwareexperiments, we show that these improvements allow the robot to ﬁnd non-myopic trajectories withhigh prediction accuracy and precision, increasing the likelihood of successful navigation (Sec. 7).
Related Works
Probabilistic Ensembles with Trajectory Sampling (PETS) [19] aims to make MBRL more robust tomodeling errors by characterizing uncertainty with probabilistic ensemble models and propogatinguncertainty with trajectory sampling.
PETS optimizes trajectories based on expected rewards andperforms well on benchmark tasks [21].
However, using PETS on rough terrain is challenging due tothe high variance in predictions, which makes it difﬁcult to estimate expected reward.
Furthermore,high reward expectation with high variance does not mean the robot will actually perform well.
Kahn et al.
[20] use uncertainty in a model-based fashion for collision avoidance in navigation.
They train an ensemble of models (via variational dropout [22]) to predict the probability of a collision.
They then use this uncertainty to augment their task-speciﬁc cost function via a risk-averseprediction of the collision probability, which factors in the uncertainty of the collision predictor ensemble.
This collision probability is used in their reward function, which is solved using MPC.
Thisapproach is shown to be effective with real-world experiments.
Recent work by Yu et al.
[23] takes a more general approach to uncertainty-aware MBRL by learningan ensemble of dynamics models and augmenting arbitrary cost functions with a scaled penaltybased on the uncertainty of the ensemble.
The authors show that such an approach allows them toperform ofﬂine reinforcement learning, or reinforcement learning without access to the environment.
Method Overview
We formulate the rough terrain navigation problem as a Markov decision process (MDP).
Our problem is deﬁned as ( S , A , P , C, γ ) , where S is the state space, A is the action space, P ( s (cid:48) | s, a ) isthe non-deterministic discrete time transition dynamics, C ( s, a ) is the cost function, and γ is thediscount factor.
We assume that the state includes terrain features (e.g. terrain height map).
Our approach, Fig. 1, falls under the MBRL paradigm.
We train a probabilistic neural networkto model the unknown transition dynamics P using previously collected data D = { d 0 , ..., d M } ,where d i =
( s 0 , .., s T , a 0 , ..., a T − 1 ) is a state-action trajectory.
We train the model to capture bothaleatoric and epistemic uncertainty using a multistep training loss that considers how model uncertainty propagates along trajectories (Sec. 4).
The trained probabilistic model is used to predict adistribution of the robot’s trajectory under the inﬂuence of a tracking controller (Sec. 5).
Finally,a low cost trajectory to the goal is generated using constrained optimization.
The solution is constrained based on the probabilistic model to avoid steps where the dynamics signiﬁcantly divergesto ensure that the tracking controller can actually achieve the trajectory (Sec. 6).
Probabilistic Dynamics Models
We propose a method for training a probabilistic dynamics model to predict long horizon trajectories.
Similar to [19], we train the model to capture both epistemic and aleatoric uncertainty, however herewe propose a multistep loss that considers how prediction uncertainty propagates along a trajectory.
4.1 Aleatoric Uncertainty
In the learned model we aim to capture aleatoric uncertainty by modeling the true transition dynamics P with a probabilistic neural network [24] parameterized by θ .
Given a state s t and action a tat time t , the model predicts a probability distribution of possible next states.
For our implementation, the network predicts the mean and log-variance of an uncorrelated gaussian distribution.
Theestimated likelihood of a next state s t +1 conditioned on s t and a t is denoted as p θ ( s t +1 | s t , a t ) .
Thetraining method in [19] uses a single step likelihood loss.
However, this loss fails to consider howprediction uncertainty propagates when multiple single step predictions are chained for trajectoryprediction.
Furthermore, [25, 26] describe how states generated from earlier predictions may fallout of the distribution of training data, leading to poor predictions for later steps in a trajectory.
Weadapt [25, 26] to non-deterministic environments by proposing a probabilistic multistep loss thatconsiders the propagation of model uncertainty instead of model error.
The multistep loss we use aims to minimize the negative log likelihood of states conditioned on allprior actions and the starting state of the trajectory, i.e. L ms ( s t , A, θ ) := − log p θ ( s t | A ) , where
A = { s 0 , a 0 , ..., a t − 1 } .
The likelihood of states s 1 , ..., s t conditioned on A can be found as follows:
[Equation]
Calculating L ms exactly is intractable due to the expectations required.
Instead we deﬁne upperbounds that can be used during training instead, denoted as L Nms where N ∈ N ,
[Equation]
where ˆ s ij is sampled recursively from p θ ( s j |
ˆ s ij − 1 , a j − 1 ) for j < t .
As N approaches inﬁnity, L Nmsapproaches the true multistep loss, L ms .
Furthermore, due to Jensen’s inequality:
[Equation]
As such, minimizing L Nms using any stochastic optimization methods such as Stochastic Gradient
Descent (SGD) [27] will effectively also minimize the true loss.
Any value could be used for N ,however larger values give more realistic estimates at the expense of increased computation.
4.2 Epistemic Uncertainty
Epistemic uncertainty is deﬁned as model uncertainty due to a poorly distributed or lack of trainingdata.
In the context of deep learning, it is typically quantiﬁed using a prediction disagreement metric via an ensemble of neural network models.
Some popular methods use explicit ensembles ofmodels [19], while other methods such as Bayesian neural networks [28] or hypernetworks [29, 30]parameterize models via uncertainty in the weights of the neural network.
In our work, we use variational dropout to characterize epistemic uncertainty [31, 22].
With thisapproach, inputs to the neural network’s fully connected layers are randomly dropped (set to zero)with some probability.
We deﬁne Θ as the parameters of the neural network dynamics model withno dropout applied.
Using dropout on this model will effectively result in a distribution of models.
We denote the probability of sampling a model, parameterized by θ , as q Θ ( θ ) .
During training, we randomly sample parameters θ from q Θ ( θ ) and calculate the multistep trainingloss described in Sec. 4.1.
We then update Θ to minimize training loss.
The complete model trainingprocedure is summarized in Algorithm 1.
Closed-Loop Trajectory Prediction
Imprecise trajectory predictions are problematic for two reasons.
First, predicted trajectory distributions with high variance provides little insight into the robot’s true outcome.
Second, the standarderror of Monte Carlo predictions scales linearly with prediction variance and inversely with thesquare root of number of samples.
This means that using Trajectory Sampling [19] may require anintractable number of samples when prediction variance is high.
We observe that it is common touse a low-level trajectory tracking controllers to correct for deviations of the true trajectory from thedesired (predicted) trajectory.
Our method aims to predict robot trajectories under the inﬂuence of alow-level trajectory tracker controllers.
Doing so allows prediction of less divergent robot trajectorydistributions and better captures the capabilities of the closed-loop robot control system.
We assume that the controllers take the form:
[Equation]
where s 0 , ..., s t are the robot’s actual states, and ¯ d = (¯ s 0 , ..., ¯ s T , ¯ a 0 , ..., ¯ a T − 1 ) is a reference trajectory containing reference states and feed forward actions.
a t is the combination of the planned,feed-forward action ¯ a t and the corrective action applied to the robot to track the reference trajectory.
Training a dynamics model to directly predict the robot’s closed loop behavior is difﬁcult as thecontroller’s inputs are high dimensional and unbounded.
Instead, our method ﬁrst predicts a nominalreference trajectory given a starting state and feed forward actions,
[Equation]
Our model outputs the predicted distribution’s mean which is the expected value above.
A distribution of closed-loop trajectories is then predicted by propagating a set of particles forward in time using the probabilistic dynamics model from Section 4.
At each time step, correctiveactions for each particle is calculated using f track based on the predicted nominal reference trajectory and the feed forward actions.
We denote the predicted closed-loop trajectory distribution asˆ D
= { ˆ d 0 , ..., ˆ d I } , where ˆ d i =
(ˆ s i 0 , ..., ˆ s i T , ˆ a i 0 , ..., ˆ a iT − 1 ) is a particle’s state-action trajectory.
Theprocedure for predicting closed-loop trajectories is summarized in Algorithm 2.
Divergence Constrained Optimization
In MBRL, low cost trajectories are found via trajectory optimization through the learned model.
However, naively minimizing cost allows the optimizer to exploit modeling errors, resulting in a trajectory with low predicted cost, but high cost under the true dynamics.
Previous work [23] preventedthe optimizer from exploiting modeling errors by penalizing trajectories with high prediction uncertainty.
This approach may be problematic as the penalty opposes the cost objective.
For example, ifa robot remains stationary, its trajectory has low prediction uncertainty, but high task cost.
Instead of penalizing uncertainty, our method uses constrained optimization to ﬁnd low cost trajectories with divergence bounded by some U , as trajectory trackers are usually effective as longas the true trajectory does not diverge too much from the reference.
Values for U are dependanton the trajectory tracker used. In our experiments, we selected U based on the tracker’s observedcapabilities.
for number of epochs do
L ←for training batch size( b ) do θ ∼ p Θ ( θ )
sample A = { s 0 , a 0 , ..., a T − 1 } ˆ s 10 , ...
ˆ s N 0 ← sfor t ∈ [1 , T ] do for i ∈ [1 , N ] do ˆ s it ∼ p
θ ( s t | ˆ
s it − 1 , a t − 1 ) p ← 1 N (cid:80) Ni =1 p θ ( s it | ˆ s it − 1 , a t − 1 )
L ← L − 1 T log p Θ ← update using gradient ∇ Θ L b
Input: Starting State s 0 ,
Feed-Forward Actions ¯ a 0 , ..., ¯ a T −// Predict Nominal Trajectory ¯ s 0 ← sfor t ∈ [1 , T ] do ¯ s t ← E [ s t ∼ p Θ ( s t | ¯ s t − 1 , ¯ a t − 1 )] // Closed-Loop Prediction for i ∈ [1 , I ] do θ i ∼ p Θ ( θ )
ˆ s i 0 , ˆ a i
← s 0 , ¯ afor t ∈ [1 , T ] do ˆ s it ∼ p
θ i (
s t | ˆ s it − 1 , ˆ a it − 1 )]
ˆ a it ← ¯ a t + f track
ˆ s i 0 , ..., ˆ s it , ¯ d )
Input: Starting State s¯ a 0 , ..., ¯ a T − 1 ← random initialization for number of Augmented Lagrangian iterations do for number of gradient descent iterations do ¯ d, ˆ D ← predict nominal and closed-loop trajectories using Algorithmu ← max t (cid:80) i (cid:107) ˆ s it − ¯ s t (cid:107)
L ← c ( ¯ d ) + λ max ( u − U, 0) a 0 , ..., a T − 1 ← update using gradient ∇ a 0 ,...,a T − 1 L Increase λ
Given a nominal trajectory ¯ d and predicted closed-loop trajectory distribution ˆ D , we deﬁne divergence as follows:
[Equation]
The constrained optimization problem we aim to solve is deﬁned as follows:
[Equation]
We use an Augmented Lagrangian optimizer to solve (7), as shown in Algorithm 3.
During optimization, the estimated divergence is thresholded, max [ u − U, 0] , scaled by a penalty factor λ , andadded to the nominal trajectory cost.
λ starts small, but is progressively increased.
Experiments
We compare our navigation method against others in simulation and in the real world.
We used PyBullet [32] for our simulation environment and a MuSHR robot [33] for our real world experiment.
Simulation training data was collected using random actions (Ornstein–Uhlenbeck noise) in randomenvironments (similar to Figure 3).
Real world data was collected using human selected actions(with Gaussian noise) in one environment (similar to Figure 4a with different arrangement). Actionscorresponded to motor commands (throttle and steering).
Training data consisted of terrain heightmaps and state-action trajectories. Simulation states included pose (in SE (3) ) and body velocity( R 6 twist vector). Real world states included pose (in SE (2) ) estimated using motion capture.
We trained the dynamics models to predict state transition probability, given an action and an observation.
Simulation observations included robot tilt ( R 3 gravity vector) and body velocity ( R 6 twistvector).
State transitions consisted of change in pose (in SE (3) ), and new body velocity.
Real worldobservations included previous state transition.
State transitions included change in pose (in SE (2) ). [Figure]
All observations also included a robot centric terrain height map cropped from the world map.
Trajectories were predicted by iteratively calculating observations given current states, sampling statetransitions from model predictions, and transitioning current states to next states. The models weretrained using our proposed multistep loss, L 128 ms , with Algorithm 1 using 5 step prediction horizons. We compared three different trajectory optimization frameworks: 1) an optimizer that only minimizes the goal cost, C ( ¯ d ) , 2) an optimizer that adds a scaled divergence penalty, C ( ¯ d ) + λu ( ¯ d, ˆ D ) ,and 3) the proposed divergence constrained optimizer (Algorithm 3). We used squared distance tothe goal as optimization and evaluation cost.
The number of optimization gradient steps were keptconsistent between methods. We tracked optimized trajectories using a simple PD controller (detailed deﬁnition of f track provided in Sec. B). The effects of the tracker were considering during optimization using closed-loop predictions (Algorithm 2). We treated the threshold U for constrainedoptimization as a hyperparameter, which we adjusted based on the observed abilities of the tracker.
Trajectory Prediction Accuracy
In simulation, we evaluated the effects of using a multistep loss and making closed-loop predictionson trajectory prediction accuracy. We trained one model using a single-step log-likelihood loss,and another model using our proposed multistep loss, L 128 ms , with Algorithm 1.
We used these twomodels to estimate the probability distribution of the robot’s state at the end of a 32 step trajectoryby propagating 512 particles using the trained models and averaging the predicted distribution fromall particles.
We simulated the robot’s true trajectory and recorded the log-likelihood of the truestate given the estimated distributions.
We repeated this process with the closed-loop trajectorypredictions using Algorithm 2.
The average log-likelihoods over 100 trials are listed in Table 1,which shows an improvement from both the multistep loss and the tracking controller.
The truetrajectories and some example particle trajectories are shown in Fig.
Navigating Over Simulated Terrain
We compared the performance of different navigation methods for the task of driving over randomlygenerated terrain with large obstacles to a goal 7 m away.
Besides the three aforementioned optimization frameworks, we also included an A* path planning method with tracking using the terraincost from [34], which considered local terrain smoothness, slope, and curvature.
We also evaluatedthe sensitivity of the optimizers to trajectory prediction accuracy by not performing closed-loop predictions.
For 250 trials, Table 2 shows each method’s success rate, deﬁned as ending within 0 .
5 mof the goal.
Table 3 shows the actual cost and standard deviation for each method with tracking.
Overall, we found that the constrained optimization has the highest success rate and lowest ﬁnalcost. Furthermore, seeding the constrained optimization with the planner resulted in an even higher [Table] [Table]
[Table]
success rate (88%), suggesting that future work should address issues with local minima.
As expected, the penalty method has the lowest divergence since it directly minimizes it, however this didnot translate to higher success rate.
We also found that prediction accuracy was especially importantto uncertainty aware optimization methods. Not including tracking during prediction signiﬁcantlydecreased the success rate of both the penalty and constrained optimization methods. Figure 3 shows one example trial.
When divergence is not considered, the optimizer exploits prediction error and ﬁnds a risky solution with high divergence.
When executed, the robot collides with anobstacle and rolls over.
Conversely, the penalty method ﬁnds an overly conservative trajectory sincethe optimizer minimizes divergence at the expense of task cost. The trajectory has low divergence,but does not reach the goal.
The divergence constrained method strikes a middle ground ﬁndinga safe low-divergence trajectory to the goal.
This behavior was exhibited throughout most of ourexperiments.
Sec E shows more trials and examples.
Real World Navigation
We also compared the three optimization frameworks in the real world, Fig. 4. The robot was taskedwith navigating across an obstacle ﬁeld to a goal 5 m away.
The ﬁeld consistent of a traversable shortobstacle, a non-traversable tall obstacle, and a ramp whose traversability depended on the robot’smaneuver.
Table 4 shows the actual cost and success rate for 10 trials.
When no uncertainty was considered, the optimizer exploited nominal prediction errors and foundimpossible trajectories through the tall obstacle.
For 9 out of 10 trials, the robot got stuck on the tallobstacle.
For 1 trial, the true trajectory deviated from the nominal enough for the robot to catch theedge of the ramp and make it to the goal.
When penalizing divergence, the optimizer avoided theramp 6 times to minimize divergence.
When using divergence constrained optimization, the robotmade it over the ramp 8 times, and on average got closer to the goal.
Discussion & Conclusion
We propose a novel MBRL algorithm for rough terrain traversal.
The main advantage of our methodis its ability to accurately optimize long horizon trajectories and thus perform non-myopic decisionmaking.
Our method uses divergence constrained optimization to ﬁnd trajectories with low divergence, ensuring that the robot can faithfully execute the trajectory.
However, the beneﬁts of usingdivergence constrained optimization are less apparent when trajectory predictions are inaccurate.
Therefore, our method trains dynamics models to consider uncertainty propagation and uses thosemodels to predict closed-loop trajectories. [Table]
Actual Trajectory Cost (Squared distance to goal) [Table] [Figure] [Figure]
Additionally, our method is practical to implement and scalable. First, our method lends itself toofﬂine settings where the agent is unable interact with the environment online. The divergenceconstrained optimization naturally avoids actions that result in high epistemic uncertainty. As aresult, the robot’s policy only performs maneuvers that are well supported by the dataset.
Second,our algorithm is computationally feasible. Although multiple trajectory predictions are required topropagate uncertainty, this process is easily parallelized for GPU computation. Furthermore, ourapproach can allow for a much longer horizon when using model predictive control and thus moretime for optimization. In our experiments, the algorithm could accurate optimize trajectories withhorizons long enough (roughly 100 steps) to not necessitate model predictive control.
Lastly, ourmethod is highly modular. Since the dynamics model is agnostic to the tracker used and task’srewards, these components can be changed online without the need to retrain the model. In the future, improvements to the constrained optimization could increase performance further. Inour experiments, the constrained optimization’s solution reached a local minimum and that seedingthe optimization with trajectories from a planner resulted in higher success.
Besides seeding, theoptimization could potentially be improved with the use of motion primitives. In the experiments, robot observations included robot-centric terrain height maps cropped fromworld height maps. We believe the assumption of world height map knowledge is reasonable sinceit is common to either build maps a priori or online from sensor data. However, these maps areoften imperfect, especially in areas with sparse sensor coverage.
Future work will incorporate mapuncertainty into trajectory predictions. Finally, we believe that this approach could also be useful for other tasks. Sec. A describes how thealgorithm could be extended to any Partially Observable Markov Decision Process.
This could beespecially promising for tasks with stochastic dynamics that require non-myopic decision making.