Application of Near Global Optimization Methods to Receding HorizonControl of Unmanned Ground Vehicles on 3D Unstructured Terrain

Gowtham Garimella1, Subhransu Mishra1, Marin Kobilarov1

Abstract— In this paper, we show the results of applyingstochastic sampling methods such as Cross Entropy method andSampling Based Differential Dynamic Programming methodto path planning of Unmanned Ground Vehicles (UGV) onunstructured 3D terrains. We show the superiority of samplingalgorithms to local optimization methods such as Gauss Newtonoptimization method through simulations.A high fidelity physicsengine model has been used to provide sample trajectorieswhich account for terrain effects such as roll over and tireslip.

I. INTRODUCTION

This work considers autonomous navigation of unmannedground vehicles (UGVs) on rough unstructured terrains. Wespecifically focus on high-frequency physics-based motiongeneration and control, i.e. computing agile forward motionsfor the next 0-10 seconds, as opposed to longer-horizonplanning. High-fidelity 3D simulation of fast wheeled ve-hicles on rough terrains has, until recently, been consideredtoo computationally expensive for real-time model predictivecontrol. Rough-terrain mobility is a well-studied topic [4],traditionally addressed by compressing the sensed terraininto a planar traversability map as opposed to a high-fidelity 3D deformable mesh capable of simulating tire-soil interaction. The traversability approach enables efficientplanning through simplified vehicle models [3], [12] evenat high speeds [14]. Typically this approach is integratedwith global long-horizon 2D map planning [10]. Recentadvances in efficient physics- based simulation could nowenable real-time 3d simulation-based control [5] for safelytraversing as opposed to simply avoiding unstructured terrain.While high-resolution particulate terrain models [13], [11]might still not be fast enough for real-time optimal control,deformable mesh models with adjustable stiffness, damping,and slip parameters are available and support faster thanreal-time simulation. Current simulation tools such as theBullet physics engine [1] offer such functionalities and,coupled with parameter identification and predictive control,could provide significant advances over the planar surfaceassumption to enable safe off-road traversal and collisionavoidance.

II. MODEL PREDICTIVE CONTROL

A. UGV Model

The model used in this work is based on a ray-castmodel using Bullet physics engine [1].This model has been

1 Gowtham Garimella, Subhransu Mishra and Marin Kobilarovare with the Department of Mechanical Engineering, Johns Hop-kins University, 3400 N Charles Str, Baltimore, MD 21218, USAggarime1|smishra9|marin@jhu.edu

Fig. 1. a) The JHU 1/5-scale model Unmanned Ground Vehicle (UGV)b) traversing an uneven terrain with obstacles; c) an optimized path to agoal location using receding horizon control (RHC) based on Bullet physicssimulation using a terrain from dense visual 3D reconstruction.

shown [6], [2] to replicate the actual car model closely wheninitialized with appropriate model parameters. The physicscan generate trajectories efficiently for example a trajectory10 seconds long running at 100 Hz can be produced in 10milliseconds.

B. Optimal Control Formulation

The goal of the optimization problem is to plan an optimaltrajectory for UGV model to reach a desired target statewhile avoiding obstacles and minimizing control effort inthe process. This is a challenging problem, since we donot have an analytical model for the rccar and the terrainis unstructured. The RHC optimization problem is stated as

Fig. 2. Tile of Pictures showing optimal trajectory for different optimization algorithms at N = 15 iterations. These figures show that CE outperformsSDDP and GN in finding nearly globally optimal trajectory.

the minimization

minξ0:n−1

ln(xn) +

n−1∑i=0

li(xi, ξi), subject to:

ξi ∈ ξ, ui = ψ(ξi, xi), xi+1 = sim(xi, ui),

(1)

where ξ ⊂ R2 denotes the admissible control set, xi are thediscrete states along the trajectory, ui are the physics enginecontrol inputs, and sim is the physics engine step function.We employ a linear quadratic cost that takes into accountcontrol cost, state cost and terminal cost to drive the trajec-tory towards the desired goal xf while penalizing controleffort. the proposed formulation can handle both followingreference trajectories {xr0, xr1, . . . } with associated controls{ξr0, ξr1, . . . } as well as reaching a single desired goal statexf :

li(xi, ξi) =1

2((xi − xri)tqi(xi − xri)

+ (ξi − ξri)tri(ξi − ξri)) (2)

ln(xn) =1

2(xn − xf )tqf (xn − xf ) (3)

qi ≥ 0; qf ≥ 0; ri > 0

When the UGV collides with obstacles, it automatically failsto reach the goal state. Hence, the simple cost formulationdescribed in (1) already accounts for obstacle avoidance,slip, and roll-over assuming the models are accurate. Thiscontrol formulation is still prone to local minima arounduntraversable obstacles such as walls, as well as highlyirregular terrain. We address this challenge using a stochasticsampling-based optimization methods such as Cross Entropy(CE) [7] based sampling and Sampling based DifferentialDynamic Programming (SDDP) [8], [9].

III. SIMULATION RESULTS AND EMPIRICAL ANALYSIS

In this section, the results of applying two stochasticsampling methods to the optimization problem described insection II-B are shown. One of the limitations of the localoptimization methods is that, the cost of the final optimaltrajectory depends on availability of a good initial guess.Figure 3 shows the average optimal trajectory cost and itsvariance with respect to the random initializations(randomξ0:n−1) for different optimization algorithms. It has beenobserved that the sampling based methods obtain a muchlower average and variance optimal cost compared to Gauss

Newton (GN) optimization method which is a local opti-mization method. The results of applying the optimization

Fig. 3. Plot shows mean and Variance of the trajectory cost for variousinitial control guesses for the same optimization problem

algorithms to an example scenario is shown in Figure 2.It can be seen that both the stochastic sampling methodsfind feasible trajectories whereas GN method fails to find afeasible trajectory to the target state.

Fig. 4. Plots show the trajectory cost for different algorithms as a functionof number of samples. GN method gets stuck in local minimum whereasSDDP and CE are able to find nearly optimal trajectories.

IV. CONCLUSIONS

In this paper, an optimal control formulation for RecedingHorizon Control of UGV on unstructured terrains has beenpresented. The superiority of stochastic sampling methodsto local optimization methods has been shown throughsimulations.

REFERENCES

[1] Bullet Physics Library.urlhttp://bulletphysics.org/.

[2] Thomas M. Howard, Colin J. Green, and Alonzo Kelly. Recedinghorizon model-predictive control for mobile robot navigation of in-tricate paths. In Field and Service Robotics, pages 69–78. Springer,2010.

[3] ThomasM. Howard, ColinJ. Green, and Alonzo Kelly. Receding hori-zon model-predictive control for mobile robot navigation of intricatepaths. In Andrew Howard, Karl Iagnemma, and Alonzo Kelly, editors,Field and Service Robotics, volume 62 of Springer Tracts in AdvancedRobotics, pages 69–78. Springer Berlin Heidelberg, 2010.

[4] Karl Iagnemma and Steven Dubowsky. Rough terrain control. InMobile Robots in Rough Terrain, pages 81–96. Springer Berlin Hei-delberg, 2004.

[5] Nima Keivan and Gabe Sibley. Realtime simulation-in-the-loop controlfor agile ground vehicles. In TAROS, 2014.

[6] Steven Lovegrove Nima Keivan and Gabe Sibley. A holistic frameworkfor planning, real-time control and model learning for high-speedground vehicle navigation over rough 3d terrain. IROS, 2012.

[7] M. Kobilarov. Cross-entropy motion planning. International Journalof Robotics Research, 31(7):855–871, 2012.

[8] Sergey Levine and Pieter Abbeel. Learning neural network policieswith guided policy search under unknown dynamics. In Advances inNeural Information Processing Systems, pages 1071–1079, 2014.

[9] Weiwei Li and Emanuel Todorov. Iterative linear quadratic regulatordesign for nonlinear biological movement systems. In ICINCO (1),pages 222–229, 2004.

[10] Maxim Likhachev, Dave Ferguson, Geoff Gordon, Anthony Stentz,and Sebastian Thrun. Anytime search in dynamic graphs. Artif. Intell.,172(14):1613–1643, September 2008.

[11] Seidl A. Madsen, J. and D. Negrut. Compaction-based deformable ter-rain model as an interface for real-time vehicle dynamics simulations.In SAE Technical Paper, 2013.

[12] Matthew Spenko, Yoji Kuroda, Steven Dubowsky, and Karl Iagnemma.Hazard avoidance for high-speed mobile robots in rough terrain.Journal of Field Robotics, 23(5):311–331, 2006.

[13] Alessandro Tasora, Dan Negrut, and Mihai Anitescu. Gpu-basedparallel computing for the simulation of complex multibody systemswith unilateral and bilateral constraints: An overview. In KrzysztofArczewski, Wojciech Blajer, Janusz Fraczek, and Marek Wojtyra,editors, Multibody Dynamics, volume 23 of Computational Methodsin Applied Sciences, pages 283–307. Springer Netherlands, 2011.

[14] E Velenis and P Tsiotras. Minimum time vs maximum exit velocitypath optimization during cornering. 2005 IEEE international sympo-sium on industrial electronics, pages 355–360, 2005.