Sampling Based Model Predictive Control withApplication to Autonomous Vehicle Guidance

Damion D. Dunlap(850) 410-6389

dunlap@eng.fsu.edu

Emmanuel G. Collins Jr.(850) 410-6373

ecollins@eng.fsu.edu

Charmane V. Caldwell(850) 410-6389

cvcaldwe@eng.fsu.eduCenter for Intelligent Systems, Control and Robotics (CISCOR)

Florida A&M University - Florida State University College of Engineering (FAMU-FSU COE)2525 Pottsdamer St. Tallahassee, FL 32310

ABSTRACTModel Predictive Control (MPC) was originally developedfor relatively slow processes in the petroleum and chemicalindustries and is well known to have difficulties in computingcontrol inputs in real time for processes with fast dynamics.In this paper a novel method called Sampling Based ModelPredictive Control (SBMPC) is proposed as a resolutioncomplete MPC algorithm to generate control inputs andsystem trajectories. The algorithm combines the benefits ofsampling based motion planning with MPC while avoidingsome of the major pitfalls facing traditional sampling basedplanning algorithms. The method is based on sampling(i.e., discretizing) the control space at each sample periodand using a goal oriented optimization method such asA∗ in place of standard linear or nonlinear programmingor evolutionary algorithms. This formulation of MPCapplies easily to systems with nonlinear dynamics andavoids the local minima which can limit the performance ofMPC algorithms implemented using nonlinear programmingalgorithms. Simulation results demonstrate the efficacy ofSBMPC in guiding an autonomous ground vehicle from astart posture to a goal posture in regions populated withobstacles.

KeywordsModel Predictive Control, Nonlinear Systems, Sampling,Fast Method

1. INTRODUCTIONIntroduced in the late 1970’s, Model Predictive Control(MPC) was developed as a means to systematically in-corporate physical and performance constraints into thecontroller formulation. Due to the fact that traditional MPCgenerates control updates relatively slowly, MPC has beenmostly relegated to the industrial process control industry,where it has seen significant success [8]. Autonomousvehicle guidance can be posed as a kinodynamic motionplanning problem, which is defined as solving for an optimaltrajectory between a start and an end state while avoidingobstacles and respecting a system model as well as inputand state constraints [2]; it is an ideal application for MPC.In order to make MPC applicable to these faster systems,the speed of MPC has to be significantly increased.

There are researchers that have employed MPC for trajec-tory generation and control of autonomous vehicles. One

method [9] uses mixed-integer programming and a processcalled “cost-to-go” to reduce the prediction horizon andapproximate the remainder of the path using a graph search.Each vehicle is modeled as a simple linear point mass withconstraints on the speed and turn rate. This method hassome relationship to the approach in this paper. Howeverthere are key differences revealed. The approach of [11]allows for nonlinear vehicle models and uses MPC coupledwith the potential field method to allow the vehicle tomaneuver around obstacles and other autonomous vehicles.The approach in this paper does not rely on potential fieldsand hence is expected to yield trajectories with lower costs.

The integration of sampling based methods into MPC hasthe potential to further improve the computational speed ofMPC. Sampling the set of viable control decisions, effectivelyreduces the size of the optimization problem while implicitlyregarding input constraints through selection of the sampledregion.

Initial sampling based planners were extensions of discretealgorithms like best-first search and A?. The randomizedpotential field method uses random walks in an attemptto guide best-first search out of local minima [1, 4]. Themethod unfortunately contained quite a few parameters thathad to be properly tuned for each new problem, but it didinspire other sampling-based planning methods [5].

Rapidly exploring random trees (RRTs) were developed asan incremental sampling method that yields good resultsin practice without any parameter tuning. This is accom-plished by incrementally constructing a search tree thatgradually improves the resolution without explicitly settingany resolution parameters. The tree will eventually coverthe free space, Cfree, generating a path that regards theconstraints. The two major limitations to RRTs as well asall sampling-based motion planners are determining whichnode is closest to the current tree and which system inputscan be used to make the connection. Determining theclosest node involves executing a nearest neighbor (NN)search while the second limitation requires the solutionor approximation of a two-point boundary value problem(BVP). There are efficient ways to approximate a NNsearch but the complexity of the BVP increases significantlywith the use of more realistic system models. Variousadaptations of the RRT methodology have been createdto deal specifically with nonholonomic constraints, dynamic

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008

obstacles as well as to heuristically bias the RRT growth [13,15, 16].

This paper introduces Sampling Based Model PredictiveControl (SBMPC) that is able to use a nonlinear dynamicmodel, take into account state and input constraints, com-pute a system trajectory and the associated control inputsfor fast systems. The key difference from previous “fast”MPC methods is that the input space is sampled at eachsample instant and a goal directed optimization algorithmis used instead of linear or nonlinear programming, or globalsearch methods such as genetic algorithms. SBMPC replacesthe potentially costly BVP of traditional sampling methodswith integration by sampling in the system input space. Thisalso has the effect of eliminating the need for the NN searchdue to the fact that the sampled input is applied directly toa current node.

This paper is arranged as follows. Section 2 provides thenecessary background to SBMPC. Section 3 presents theSBMPC method. Section 4 presents simulation results. Sec-tion 5 discusses the results and makes suggestions regardingfuture work. Finally Section 6 presents the conclusions.

2. BACKGROUNDThe section formulates the kinodynamic motion planningproblem and presents the standard principles of MPC, es-tablishing the framework from which SBMPC is constructed.

2.1 The Kinodynamic Motion Planning Prob-lem

A great number of autonomous vehicles utilize an architec-ture wherein the vehicle guidance task is separated into amotion planning algorithm that generates a trajectory anda feedback controller that tracks the generated trajectory.These systems neglect the fact that most of the motionplanning algorithms in current literature do not easily incor-porate kinematic or dynamic models [12]. The kinodynamicmotion planning problem was formulated to facilitate thedevelopment of motion planning algorithms that can moreeasily reflect the same types of constraints to which thephysical systems are subject [2].

Although the kinodynamic motion planning problem is notrestricted to a particular type of system, we will assumethat the system is the discrete, multi-input multi-output(MIMO), nonlinear system:

x(k + 1) = f (x(k), u(k), w(k)) (1)

ym(k) = g (x(k)) + ξ(k) (2)

where x(k) is the state, u(k) is the control input, w(k)is the unmeasured disturbance, ym(k) is the measuredoutput and ξ(k) is the measurement noise. Given thedifficulty of the problem, algorithms that aim for real-time performance must pursue an approximate solution.Accepting an approximate solution necessitates a tradeoffbetween planning time and optimality, where optimality,although often considered to be minimum time, can alsobe expressed in other terms such as energy, path length, orcloseness to the desired goal state. Given the complexitylimitation of practical application, the concepts of sampling

were well suited for application in the initial kinodynamicmotion planning algorithms.

2.2 Model Predictive ControlModel Predictive Control generally works by solving anoptimization problem at every time step k that determinescontrol inputs for the next N steps, which is known asthe prediction horizon. This optimal control sequenceis determined by using the system model to predict thepotential system response, which is then evaluated by thecost function J . Most commonly, a quadratic cost functionminimizes control effort as well as the error between thepredicted and a reference trajectory, r. The prediction andoptimization operate together to generate sequences of thecontrol output u and the resulting system output y.

The various forms of MPC, can generally be classified aseither receding horizon [7] or shrinking horizon [6, 14].Receding Horizon MPC (RHMPC) is primarily used to tracka reference trajectory, which usually leads to a set point,while the lesser known Shrinking Horizon MPC (SHMPC)enables the algorithm to generate the trajectory that leadsto a given set point or goal region. The latter of theseapproaches is most applicable to the kinodynamic motionplanning problem.

2.2.1 Receding Horizon Model Predictive ControlFor RHMPC the optimization problem is typically posed as:

min J =

N∑i=1

‖r(k + i)− y(k + i)‖2Q(i) +

M−1∑i=0

‖u(k + i)‖2S(i)

(3)subject to the model constraints,

x(k + i) = f (x(k + i− 1), u(k + i− 1)) , (4)

y(k + i) = g (x(k + i)) + β(k), (5)

and the inequality constraints,

Ax ≤ bC(x) ≤ 0

ul ≤ u(k + i) ≤ uu,(6)

where the prediction and control horizons are N and Mrespectively, β(k) is a bias expression that compares thecurrent predicted output y(k) to the current measuredoutput ym(k), C(x) represents the nonlinear constraintson the states, and Q(i) ≥ 0 and S(i) ≥ 0 are the errorand control effort weights respectively. The first optimalcontrol input is implemented after which the process isrepeated until the goal is achieved. In RHMPC the fixed sizeprediction window moves at each time step, as illustrated inFig. 1. The objective is for the predicted output y(k+ i), toreach the reference trajectory r(k+i), utilizing the proposedinputs u(k + i) during the prediction horizon N .

2.2.2 Shrinking Horizon Model Predictive ControlSHMPC differs from RHMPC in that it employs a varyingsize horizon that terminates at the end state. As illustratedin Fig. 2, the prediction horizon“shrinks”as the horizon sizedecreases by one time interval at each step. In this method,a trajectory is generated that must reach the goal state, G,yielding the optimization problem,

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008

Figure 1: Receding Horizon Model PredictiveControl Concept (N = 3)

Figure 2: Shrinking Horizon Model PredictiveControl Concept (N∗ = 6)

min J =

N∗−k∑i=1

‖G− y(k + i)‖2Q(i) +

M−1∑i=0

‖u(k + i)‖2S(i) (7)

subject to ( 5) and ( 6), where N∗ is the endpoint, Q(i) ≥ 0and S(i) ≥ 0. The first summation of the cost functionminimizes the deviation between the predicted outputs andthe endpoint G while the second summation minimizes thecontrol effort. Since the control inputs are chosen thatminimize the distance from the goal, the trajectory andnecessary control inputs needed to reach the desired end-point are resolved simultaneously.

Since the longer prediction horizons typically required bySHMPC can be computationally expensive, RHMPC canbe made to resemble SHMPC through the use of a lowcost prediction beyond the normal prediction horizon asdepicted in Fig. 3. The approach was introduced in [10] forapplication to autonomous vehicle guidance and representsa good paradigm for speeding up SHMPC.

3. SAMPLING-BASED MODEL PREDICTIVECONTROL

Sampling-Based MPC (SBMPC) combines MPC with theconcepts of sampling-based motion planners to effectively

Figure 3: Generalization of RHMPC to SHMPC

reduce the problem size of MPC by sampling the systeminput space. The method also replaces the MPC opti-mization phase with an A∗ like algorithm. The SBMPCoptimization varies from the typical application of A∗ inthat the nodes are configurations in the state space andthe edges are comprised of the control inputs that link twostates. SBMPC retains the computational efficiency andguaranteed convergence properties of A∗ [3] while avoidingsome of the computational bottlenecks of sampling-basedmotion planners. The local planning method of sampling-based motion planners is simplified from what is effectivelya two-point boundary value problem by sampling in theinput space and integrating to determine qnew. Althoughmost sampling-based planners use proximity to a randomor voronoi biased point as their vertex selection method,the proposed method biases the vertex selection with aA∗-like heuristic eliminating the need for a potentiallycostly nearest neighbor search while promoting advancementtowards the goal. The resulting algorithm is thereforeresolution complete yet its efficiency is dependant on thequality of the heuristic function.

3.1 The SBMPC AlgorithmThe main component of the SBMPC algorithm is theoptimization, which will be called Sampling-Based ModelPredictive Optimization and consists of the following steps:

1. Sample Control Space: Generate a set of samplesof the control space that satisfy the input constraints.

2. Generate Neighbor Nodes: Integrate the systemmodel with the control samples to determine theneighbors of the current node.

3. Evaluate Node Costs: Use an A∗-like heuristic toevaluate the cost of the generated nodes based on thedesired objective (shortest distance, shortest time, orleast amount of energy, etc.). The heuristic can bedescribed as optimizing a cost function of the form:

min J =

N∗∑i=1

‖y(k + i)− y(i)‖Q (i)2 +

N∗∑i=0

‖u(i)‖S (i)2

(8)

where y(k) is the current predicted output and y(k +i) is the next predicted output. Unlike (7), the costfunction does not explicitly incorporate the goal stateG because the optimization method is goal orientedand implicitly considers the goal through minimizingthe length of each path segment.

4. Select Lowest Cost Node: The nodes are collectedin the Open List, which ranks the potential expansionnodes by their cost. The Open List is implemented asa heap so that the lowest cost node that has not beenexpanded is on top.

5. Evaluate Edge Cost for the “Best” Node: Eval-uate each of the inequality constraints described in (6)for the edge connecting the “best” node to the currentnode. The edge cost evaluation requires sub-samplingand iteration of the model with a smaller time step forincreased accuracy; it is therefore only computed for

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008

the current“best”node. In the worst case the edge costof all of the neighbor nodes will be evaluated which ishow A∗ typically computes cost.

6. Check for Constraint Violations: If a constraintviolation occurred, go back to step 4 and get the next“best” node.

7. Check for Completion: Determine if the currentsolution contains a path to the goal. If yes, stop. Ifno, go back to step 1.

The entire algorithm is integrated into the MPC frameworkby executing the first control and repeating the optimizationuntil the goal is reached since the completion of SBMPOrepresents the calculation of a path to the goal and not thecomplete traversal as seen in Algorithm 1.

Algorithm 1 Sampling Based Model Predictive Control

1: while ycurr 6= ygoal do2: while OpenList.top() < ycurr do3: nbest ⇐ OpenList.top()4: Generate Nsamples samples of the control space5: Generate Nsamples neighbor nodes6: for each neighbor node do7: Evaluate node cost8: if Node cost <∞ then9: Add node to the OpenList

10: repeat11: nbest ⇐ OpenList.top()12: Evaluate nbest edge cost13: until nbest = OpenList.top()14: ycurr ⇐ nbest

4. RESULTSIn order to demonstrate its robustness and ability to considernonholonomic constraints, the kinematic model of an Acker-man steered vehicle is employed in the following simulations.The discretized model is

x(k + 1) = x(k) + cos θ(k) · u1(k) · Ts

y(k + 1) = y(k) + sin θ(k) · u1(k) · Ts

θ(k + 1) = θ(k) +tanψ(k)

l· u1(k) · Ts

ψ(k + 1) = ψ(k) + u2(k) · Ts (9)

where u1 and u2 are the forward velocity and steering raterespectively, l is the longitudinal wheel separation, ψ andθ are the steering and orientation angles respectively, andTs is the time step. The non-holonomic constraint can beexpressed as:

x(k + 1)sinθ(k)− y(k + 1)cosθ(k) = 0 (10)

Table 1: Simulation ParametersSampling Time 1.0s

Subsampling Time 0.1sNo. of Samples per expansion 25

Velocity: u1(m/s) [−2.0 2.0]Steering Rate: u2(rad/s) [−π/2 π/2]

In order to illustrate the properties of SBMPC, the algo-rithm was applied to randomly generated scenarios with50 obstacles of various sizes. The velocity and steeringrate were constrained as indicated in Table 1 and all thescenarios begin at the origin,(0, 0) and end at (10, 10).The thin segments represent all the trajectories that wereevaluated while the thicker line represents the calculatedtrajectory. The first and second scenarios in Figs. 4 and5 are representative of typical results where the resultingpath consists of 12-19 steps with computation times below0.8s. The smoothness of the trajectory indicates how thesteering rate constraint limits the maneuverability of thevehicle, forcing it to make wider turns than would occurwithout this constraint.

Figure 4: Scenario 1: start=(0,0), end=(10,10)

The third scenario shown in Fig. 6 demonstrates the abilityof SBMPC to negotiate potential local minimum as wellas navigate narrow passages. The algorithm continues togenerate samples until a solution path is found and althoughthis does result in longer computation times, the algorithmhas never failed to generate a valid solution.

The final scenario, shown in Fig. 7, highlights two of themain problems with the current implementation of SBMPC,the most obvious being the necessity of increasing thesampling near the goal region as indicated by the densityof potential paths shown in that area. The second is thebacktracking that occurs near the middle of the path as wellas near the goal region. This occurs due to the absence ofwhat is referred to in the A∗ algorithm as a back pointer.The addition of a back pointer would enable the algorithm toidentify when multiple trajectories lead to same state. Thepath to that state can then be redefined to be the lowestcost of the paths. Future implementations of SBMPC willintegrate the back pointer which should make the algorithmmore efficient while improving the final solution.

Initial results from the SBMPC algorithm show great promiseby enabling longer planning horizons and an increased levelof scenario complexity. The objective for the final versionof this paper is to compare SBMPC with standard MPCimplemented using both a Trust-Region Newton method anda Genetic Algorithm (GA); the comparison will be based

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008

Figure 5: Scenario 2: start=(0,0), end=(10,10)

Figure 6: Scenario 3: start=(0,0), end=(10,10)

on computational performance and solution quality. Thecurrent implementations of the MPC variants are coded inMatlab while SBMPC is implemented in C++, which wouldnot offer a fair comparison. Preliminary comparison resultshave shown SBMPC’s ability to successfully negotiate sce-narios where the Newton based method gets stuck in a localminimum. The simplicity of SBMPC has also made it morecapable of obtaining quality solutions for various scenariosthan the other methods.

SBMPC effectively makes the trade off between computationtime and solution optimality but if there are not enoughsamples generated near the goal region, SBMPC can beinefficient in reaching the goal state. For example, the goalregion shown in Fig. 8 may require several more iterations toreach the goal region when the goal could have been achievedby the current step if more appropriate samples had beengenerated. The illustration was generated by evaluating allpossible control sequences of length 2 from a fixed set ofinputs and also demonstrates how this strategy could fail toreach the goal.

Figure 7: Scenario 4: start=(0,0), end=(10,10)

Figure 8: Illustration of the necessity of sufficientsampling near the goal.

5. DISCUSSIONThe current SBMPC software was designed primarily forusability and has not yet been optimized for speed. Theoptimized code is expected to yield much faster computa-tional times; in fact, given that the main computationaldifference between the SBMPC algorithm and the A? andD? algorithms involves the integration of the model, theperformance of SBMPC is not expected to be substantiallyslower than standard uses of A? and D?. The two main areasof improvement in the current software involve integration ofsome type of spatial hashing of the state or output space andsubstantial improvement of the sorting of the Open List. Toillustrate the need for spatial hashing, consider the loopingbehavior shown before the path enters the narrow passageof Figure 6. Spatial hashing would allow the algorithmto identify when a particular state is reached by twoseparate trajectories and enable the algorithm to choose thelower cost path, thereby eliminating the unnecessary pathsegments. Currently, most of the computational time is usedto sort the Open List, which can be significantly improvedthrough the use of more efficient data structures that reducethe amount of data that is actually moved around during thesorting process. In addition, fast replanning has not yet beenimplemented in the current software.

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008

The SBMPC algorithm is resolution complete (see Section3). Practically, this means that if the algorithm is allowed torun, which may involve adding more samples at some timesteps, it will find a solution if it exists. The solution willbe globally optimal for the sampling assumed. This is truewhether the model is linear or nonlinear, which is a verypowerful feature of the algorithm.

In addition, the algorithm is extremely easy to tune. Theonly significant tuning parameter is the default numberof samples at each sample period. In contrast, standardMPC requires tuning of the prediction and control horizonsand for nonlinear systems can be sensitive to the choiceof the initial input trajectory; in the experience of theauthors it is more difficult to tune in order to achievegood performance. Both methods also require that the costfunction weights be chosen. The relative effect of the weightson the two approaches is currently unclear. A comprehensivecomparison of the tuning of the two approaches is part offuture research.

6. CONCLUSIONSampling Based Model Predictive Control has been shownto effectively guide autonomous vehicles incorporating thevehicle model to determine the optimum trajectory. SBMPCexploits sampling-based concepts along with the A∗ op-timization algorithm to achieve the goal of being ableto quickly determine control updates while avoiding localminima and regarding all constraints. SBMPC addressesthe kinodynamic motion planning problems of generating atrajectory, but can also be used to follow a trajectory asin Receding Horizon MPC by selecting coincidence pointsalong the trajectory and treating each as an endpoint.

7. DISCLAIMERThe views and conclusions contained in this document arethose of the authors and should not be interpreted asrepresenting the official policies, either expressed or implied,of the Army Research Laboratory or the U. S. Government.References are important to the reader; therefore, eachcitation must be complete and correct. If at all possible,references should be commonly available publications.

8. REFERENCES[1] J. Barraquand and J. Latombe. A Monte-Carlo

algorithm for path planning with many degrees offreedom. IEEE International Conference on Robotics& Automation, pages 1712–1717, 1990.

[2] B. Donald, P. Xavier, J. Canny, and J. Reif.Kinodynamic motion planning. Jounal of theAssocation for Computing Machinery,40(5):1048–1066, November 1993.

[3] P. Hart, N. Nilsson, and B. Raphael. A formal basisfor the heuristic determination of minimum costpaths. IEEE Transactions of Systems Science andCybernetics, ssc-4(2):100–107, 1968.

[4] J. Latombe. Robot Motion Planning. Kluwer, Boston,MA, 1991,.

[5] S. M. LaValle. Planning Algorithms. CambridgeUniversity Press, 2006.

[6] V. Liotta, C. Georgias, and M. El-Aasser. Real-timeestimation and control of particle size in semi-batch

emulsion polymerization. IEEE American ControlConference, pages 1172–1176, June 1997.

[7] J. Maciejowski. Predictive Control: with Constraints.Prentice Hall, Haslow, UK, 2002.

[8] S. Qin and T. Badgwell. An overview of industrialmodel predictive control technology. Proceedings ofCPC-V, 1996.

[9] A. Richards and J. How. Aircraft trajectory planningwith collision avoidance using mixed integer linearprogramming. American Control Conference, pages1936–1941, 2002.

[10] A. Richards and J. How. Mixed-integer programmingfor control. IEEE American Control Conference, pages2677–2683, June 2005.

[11] D. Shim, J. Kim, and S. Sastry. Decentralizednonlinear model predictive control of multiple flyingrobots. IEEE Conference on Decision & Control,4:3621–3626, 2003.

[12] R. Siegwart and I. R. Nourbakhsh. Introduction toAutonomous Mobile Robots. The MIT Press, 2004.

[13] E. Szadeczky-Kardoss and B. Kiss. Extension of therapidly exploring random tree algorithm with keyconfigurations for nonholonomic motion planning.IEEE 3rd International Conference on Mechatronics,pages 363–368, 2006.

[14] M. Thomas, J. Kardos, and B. Joseph. Shrinkinghorizon model predictive control applied to autoclavecuring of composite laminate materials. IEEEAmerican Control Conference, pages 505–509, June1994.

[15] C. Urmson and R. Simmons. Approaches forheuristically biased RRT growth. InternationalConference on Intelligent Robots & Systems, pages1178–1183, 2003.

[16] M. Zucker, J. Kuffner, and M. Branicky. MultipartiteRRTs for rapid replanning in dynamic environments.IEEE International Conference on Robotics &Automation, pages 1603–1609, April 2007.

2008 Florida Conference on Recent Advances in Robotics, FCRAR 2008 Melbourne, Florida, May 8 - May 9, 2008