Robust Online Motion Planning with Regions of Finite Time...

Robust Online Motion Planning with Regions ofFinite Time Invariance

Anirudha Majumdar and Russ Tedrake

Abstract In this paper we consider the problem of generating motion plans for anonlinear dynamical system that are guaranteed to succeed despite uncertainty inthe environment, parametric model uncertainty, disturbances, and/or errors in stateestimation. Furthermore, we consider the case where these plans must be generatedonline, because constraints such as obstacles in the environment may not be knownuntil they are perceived (with a noisy sensor) at runtime. Previous work on feedbackmotion planning for nonlinear systems was limited to offline planning due to thecomputational cost of safety verification. Here we take a trajectory library approachby designing controllers that stabilize the nominal trajectories in the library and pre-computing regions of finite time invariance (“funnels”) for the resulting closed loopsystem. We leverage sums-of-squares programming in order to efficiently computefunnels which take into account bounded disturbances and uncertainty. The resultingfunnel library is then used to sequentially compose motion plans at runtime whileensuring the safety of the robot. A major advantage of the work presented here isthat by explicitly taking into account the effect of uncertainty, the robot can evaluatemotion plans based on how vulnerable they are to disturbances. We demonstrate ourmethod on a simulation of a plane flying through a two dimensional forest of polyg-onal trees with parametric uncertainty and disturbances in the form of a bounded“cross-wind”.

1 Introduction

The ability to plan and execute dynamic motions under uncertainty is a critical skillwith which we must endow our robots in order for them to perform useful tasksin the real world. Whether it is an unmanned aerial vehicle (UAV) flying at high

Anirudha Majumdar and Russ TedrakeMassachusetts Institute of Technology, 77 Massachusetts Avenue, Cambridge MA - 02139, e-mail:{[email protected], [email protected]}

1

2 Anirudha Majumdar and Russ Tedrake

X

Y

Planned

Actual

(a) A plane deviating from its nominal plannedtrajectory due to a heavy cross-wind.

X

Y

(b) The “funnel” of possible trajectories.

Fig. 1 Not accounting for uncertainty while planning motions can lead to disastrous consequences.

speeds through a cluttered environment in the presence of wind gusts, a legged robottraversing rough slippery terrain, or a micro-air vehicle with noisy on-board sensing,the inability to take into account disturbances, model uncertainty and state uncer-tainty can have disastrous consequences.

Motion planning has been the subject of significant research in the last fewdecades and has enjoyed a large degree of success in recent years. Planning al-gorithms like the Rapidly-exploring Randomized Tree (RRT) [12], RRT? [11], andrelated trajectory library approaches [13] [5] can handle large state space dimen-sions and complex differential constraints, and have been successfully demonstratedon a wide variety of hardware platforms [22] [21]. However, a significant failing istheir inability to explicitly reason about uncertainty and feedback. Modeling errors,state uncertainty and disturbances can lead to failure if the system deviates fromthe planned nominal trajectories. A cartoon of the issue is sketched in Figure 1(a),where a UAV attempting to fly through a forest with a heavy cross-wind gets blownoff its planned nominal trajectory and crashes into a tree.

More recently, planning algorithms which explicitly take into account feedbackcontrol have been proposed. LQR-Trees [24] and the minimum snap trajectory gen-eration approach [16] operate by generating locally optimal trajectories throughstate space and stabilizing them locally. However, the former approach lacks theability to handle scenarios in which the task and environment are unknown till run-time since it is too computationally intensive for online implementation. The lattertechnique has no mechanism for reasoning about uncertainty in the form of unmod-eled disturbances, state errors and model uncertainty.

Robust Online Motion Planning with Regions of Finite Time Invariance 3

In this paper, we present a partial solution to these issues by combining trajec-tory libraries, feedback control, and sums-of-squares programming [18] in order toperform robust motion planning in the face of uncertainty. In particular, in the of-fline computation stage, we design a finite library of motion primitives and augmentthem with feedback controllers that locally stabilize them. Then, using sums-of-squares programming, we compute robust regions of finite time invariance (“fun-nels”) around these trajectories that guarantee stability of the closed loop systemin the presence of sensor noise, parametric model uncertainty, unmodeled boundeddisturbances and changes in initial conditions. A cartoon of these regions of finitetime invariance, or “funnels”, is shown in Figure 1(b). Finally, we provide a way ofcomposing these robust motion plans online in order to operate in a provably safemanner.

One of the most important advantages that our approach affords us is the abilityto choose between the motion primitives in our library in a way that takes into ac-count the dynamic effects of uncertainty. Imagine a UAV flying through a forest thathas to choose between two motion primitives: a highly dynamic roll maneuver thatavoids the trees in front of the UAV by a large margin or a maneuver involves flyingstraight while avoiding the trees only by a small distance. A traditional approachthat neglects the effects of disturbances and uncertainty may prefer the former ma-neuver since it avoid the trees by a large margin and is therefore “safer”. However, amore careful consideration of the two maneuvers leads to a different conclusion: thedynamic roll maneuver is far more susceptible to wind gusts and state uncertaintythan the second one. Thus, it may be much more advantageous to execute the secondmotion primitive. Further, it may be possible that neither maneuver is guaranteed tosucceed and it is safer to abort the mission and simply transition to a hover mode.Our approach allows robots to make these critical decisions, which are essential ifrobots are to move out of labs and operate in real-world environments.

2 Related Work

The motion planning aspect of our approach draws inspiration from the vast bodyof work that is focused on computing motion primitives in the form of trajectorylibraries. For example, trajectory libraries have been used in diverse applicationssuch as humanoid balance control [13], autonomous ground vehicle navigation [21]and grasping [4]. The Maneuver Automaton [5] attempts to capture the formal prop-erties of trajectory libraries as a hybrid automaton, thus providing a nice unifyingtheoretical framework. Further theoretical investigations have focused on the offlinegeneration of diverse but sparse trajectories that ensure the robot’s ability to per-form the necessary tasks online in an efficient manner [6]. More recently, tools fromsub-modular sequence optimization have been leveraged in the optimization of thesequence and content of trajectories evaluated online [4].

Robust motion planning has also been a very active area of research in therobotics community. Early work focused on the purely kinematic problem of plan-


ning paths through configuration space with “tubes” of specified radii around themsuch that all paths in the tube remained collision-free [9] [8]. Recent work has fo-cused on reasoning more explicitly about the manner in which uncertainty/disturbancesinfluence the dynamics of the robot, and is closer in spirit to the work presentedhere. In particular, [20] approaches the problem through dynamic programming ona model with disturbances by making use of the Maneuver Automaton frameworkmentioned earlier. However, the work does not take into account obstacles in theenvironment and does not provide or make use of any explicit guarantees on al-lowed deviations from the planned trajectories in the Maneuver Automaton. Theauthors of [27] attempt to address the robust motion planning problem through H∞

control techniques. However, the results focused on a planar vehicle and the basicmotion planning framework does not easily extend to three dimensional environ-ments. Also, while regions of invariance are approximated analytically and numeri-cally, they are used only to evaluate the performance of the controller and not whileconstructing motion plans. Another approach that is closely related to ours is ModelPredictive Control with Tubes [15]. The idea is to solve the optimal control problemonline with guaranteed “tubes” that the trajectories stay in. However, the method islimited to linear systems and convex constraints.

A critical component of the work presented here is the computation of regions ofinvariance for nonlinear systems via Lyapunov functions. This idea, along with themetaphor of a “funnel”, was introduced to the robotics community in [2], where fun-nels were sequentially composed in order to produce dynamic behaviors in a robot.In recent years, sums-of-squares programming has emerged as a way of checkingthe Lyapunov function conditions [18]. The technique relies on the ability to checkpositivity of multivariate polynomials by expressing them as a sum-of-squares. Thiscan be written as a semi-definite optimization program and is amenable to efficientcomputational algorithms such as interior point methods [17]. Assuming polyno-mial dynamics of our system, one can check that a polynomial Lyapunov candidate,V (x), satisfies V (x)> 0 and V (x)< 0 in some region Br. Importantly, the same ideacan be used in computing regions of finite time invariance (“funnels”) around time-indexed trajectories of the system [25]. Further, robust regions of attraction can becomputed using an approach that verifies stability/invariance of sets around fixedpoints under parametric uncertainty [26]. In this paper, we show how to combinethese ideas to compute robust regions of finite-time invariance around trajectoriesthat guarantee that the if the system starts off in the set of given initial conditions, itwill remain in the computed “funnel” even if the model of the dynamics is uncertainand the system is subjected to bounded disturbances and state uncertainty.

3 Contributions

This paper makes two main contributions. First, we provide a way of generatingregions of finite time invariance (“funnels”) for time-varying polynomial systemssubjected to a general class of uncertainty (bounded uncertainty in parameters en-


tering polynomially in the dynamics). This is an extension of the results presentedin [25], which presents a method for computing “funnels” for systems with time-varying polynomial dynamics assuming no model uncertainty/disturbances. Second,we show how a library of such funnels can be precomputed offline and composedtogether at runtime in a receding horizon manner while ensuring that the resultingclosed loop system is “safe” (i.e. avoids obstacles and switches between the plannedsequence of funnels). This can be viewed as an extension of the LQR-Trees algo-rithm [24] for feedback motion planning, which was limited to offline planning dueto the relatively large computational cost of computing the funnels; our algorithmis suitable for real-time, online planning. We expect this framework to be useful inrobotic tasks where the dynamics and perceptual system of the robot are difficult tomodel perfectly and for which the robot does not have access to the geometry of theenvironment till runtime.

4 Approach

A considerable amount of research effort in the motion planning community hasfocused on the design of trajectory libraries (Section 2). Further, a substantialbut largely separate literature in control theory [19] [7] exists for the design ofcontrollers that stabilize uncertain non-linear systems along nominal trajectories.Hence, in this section, we do not focus on the particulars of the generation of trajec-tory libraries and controllers. Rather, we concentrate on how these powerful meth-ods from the fields of motion planning and control theory can be combined withsums-of-squares programming in order to perform robust online motion planning inthe face of dynamic and state uncertainty. To this end, we assume that we are pro-vided with a trajectory library consisting of a finite set of nominal feasible trajecto-ries for the robot and a corresponding set of controllers that stabilize these trajecto-ries. We discuss one particular method for obtaining trajectory libraries, controllersand Lyapunov functions in Section 5.

4.1 Robust Regions of Finite Time Invariance

The techniques for the computation of regions of finite time invariance presented in[25] can be extended to handle scenarios in which there is uncertainty in state anddynamics. This computation is similar to the one presented in [26], but instead ofcomputing regions of attraction for asymptotically stable equilibrium points, we askfor certificates of finite time invariance around trajectories (“funnels”). Let x0(t) :[0,T ] 7→ Rn be a trajectory in our trajectory library and u(x, t) be the controller thatstabilizes x0(t). Then, the closed loop dynamics of the system can be written as:

x = f (x, t,w(t,x)) (1)


where w(t,x) ∈W ⊂ Rd is a free (but bounded) uncertainty term that can be usedto model instantaneous disturbances, parametric model uncertainty, and state uncer-tainty.

We further assume that we have a time varying Lyapunov function, V (x, t), thatlocally guarantees stability around the trajectory. An example of how such a candi-date Lyapunov function may be obtained in practice is provided in Section 5. Givena set of initial conditions X0 = {x|V (x, t) ≤ ρ(0)}, our goal is to compute a tightouter estimate of the set of states the system may evolve to under the closed loopdynamics and bounded uncertainty. In particular, we are concerned with finding aset of states Xt = {x|V (x, t)≤ ρ(t)} for each time t ∈ [0,T ] such that:

x(0) ∈ X0 =⇒ x(t) ∈ Xt ,∀t ∈ [0,T ].

As described in [25], under mild technical conditions it is sufficient to find ρ(t) suchthat:

V (x, t) = ρ(t) =⇒ V (x, t,w)≤ ρ(t),∀w ∈W. (2)

As noted in [25], checking condition (2) on a finite set of time samples, ti, results inlarge computational gains while still maintaining accuracy. Thus, assuming that theclosed loop dynamics are polynomial in x and w for a given t, and that w belongs toa bounded semi-algebraic set W = {w|Wj(w)≥ 0}, we can write a sums-of-squaresoptimization program to compute ρ(ti) (and thus Xt ):

minimizeρ,Li,M j

∑ti

ρ(ti) (3)

subject to ρ(ti)−V (x,w)−Li(x)[V (x)−ρ(ti)]−∑j

M j(w)Wj(w)≥ 0,∀ti

M j > 0, ρ(ti)≥ 0

It is easy to see that (3) is a sufficient condition for (2) since when Wj(w) ≥ 0 andV (x) = ρ(ti), we have that ρ(ti)− V (x,w) ≥ 0. This optimization program is bi-linear in the decision variables and is amenable to an alternating search over ρ andLagrange multipliers (as in [25]). Note that the objective in our sums-of-squaresprogram, ∑ti ρ(ti), helps us find a tight conservative estimate of the set of states theclosed loop system may evolve to under the given uncertain dynamics. Further, onecan very easily augment this optimization program to handle actuator saturations ina manner similar to the one described in [24].

4.2 Funnel Libraries

The tools from Section 4.1 can be used to create libraries of funnels offline. Given atrajectory library, T , consisting of finitely many trajectories xi(t), locally stabilizingcontrollers ui(x, t), and associated candidate Lyapunov functions Vi(x, t), we cancompute a robust funnel for each trajectory in T . However, there is an important


issue that needs to be addressed when designing libraries of funnels and has ananalogy in the traditional trajectory library approach. In particular, trajectories ina traditional trajectory library need to be designed in a way that allows them tobe sequenced together. More formally, let P denote the projection operator thatprojects a state, x, onto the subspace formed by the non-cyclic dimensions of thesystem (i.e. the dimensions with respect to which the Lagrangian of the system isnot invariant). Then, for two trajectories xi(t) and x j(t) to be executed one afteranother, we must have

P(xi(Ti)) = P(x j(0)).

Note that the cyclic coordinates do not pose a problem since one can simply “shift”trajectories around in these dimensions. This issue is discussed thoroughly in [5]and is addressed by having a trim trajectory of the system that other trajectories(maneuvers) start from and end at (of course, one may also have more than one trimtrajectory).

In the case of funnel libraries, however, it is neither necessary nor sufficient forthe nominal trajectories to line up in the non-cyclic coordinates. It is the interfacebetween funnels that is important. Let xi(t) and x j(t) be two nominal trajectories inour library and Fi(t) = {x|Vi(x, t) ≤ ρi(t)} and Fj(t) = {x|Vj(x, t) ≤ ρ j(t)} be thecorresponding funnels. Further, we write x = [xc,xnc], where xc represent the cyclicdimensions and xnc the non-cyclic ones. Following Burridge and Koditschek [2], wesay that two funnels are sequentially composable if:

P(Fi(Ti))⊂P(Fj(0)) (4)⇐⇒ ∀x = [xc,xnc] ∈ Fi(Ti), ∃x0,c s.t. [x0,c,xnc] ∈ Fj(0)

While (4) is a necessary and sufficient condition for two funnels to be executed oneafter another, the dependence of x0,c on x makes searching for x0,c a non-convexproblem in general. Thus, we set x0,c to be the cyclic coordinates of x j(0), resultingin a stronger sufficient condition that can be checked via a sums-of-squares program:

∀x = [xc,xnc] ∈ Fi(Ti), [x0,c,xnc] ∈ Fj(0) (5)

Intuitively, (5) corresponds to “shifting” the inlet of funnel Fj along the cyclic di-mensions so it lines up with xc. Assuming Fi(Ti) and Fj(0) are semi-algebraic sets,a simple sums-of-squares feasibility program can be used to check this condition:

ρ j(0)−Vj(P([x j(0),xnc],0))+L(x)[Vi([xc,xnc],Ti)−ρi(Ti)]≥ 0 (6)L(x)≥ 0

where L(x) is a non-negative Lagrange multiplier. Note that not all pairs of funnelsin the library will be sequentially composable in general. Thus, as we discuss inSection 4.3, we must be careful to ensure sequential composability when planningsequences of funnels online.


4.3 Online Planning with Funnels

Having computed libraries of funnels in the offline pre-computation stage, we canproceed to use these primitives to perform robust motion planning online. Therobot’s task specification may be in terms of a goal region that must be reached(as in the case of a manipulator arm grasping an object), or in terms of a nominaldirection the robot should move in while avoiding obstacles (as in the case of a UAVflying through a forest or a legged robot walking over rough terrain). For the sakeof concreteness, we adopt the latter task specification although one can easily adaptthe contents of this section to the former specification. We further assume that therobot is provided with polytopic regions in configuration space that obstacles areguaranteed to lie in and that the robot’s sensors only provide this information up toa finite (but receding) spatial horizon. Our task is to sequentially compose funnelsfrom our library in a way that avoids obstacles while moving forwards in the nomi-nal direction. The finite horizon of the robot’s sensors along with the computationalpower at our disposal determines how long the sequence of planned funnels can beat any given time.

The most important computation that needs to be performed at runtime is tocheck whether a given funnel intersects an obstacle. For the important case in whichour Lyapunov functions are quadratic in x, this computation is a Quadratic Program(QP) and can be solved very efficiently (as evidenced by the success of Model Pre-dictive Control [3]). Let the funnel be given by the sets Xti = {x|V (x, ti) ≤ ρ(ti)}for i = 1, . . . ,N, and a particular obstacle defined by half-plane constraints A jx ≥ 0for j = 1, ...,M. Note that A j will typically be sparse since it will contain zeros inplaces corresponding to non-configuration space variables (like velocities). Then,for i = 1, . . . ,N, we solve the following QP:

minimizex

V (x, ti) (7)

subject to A jx≥ 0,∀ j

Denoting the solution of (7) for a given ti as V ?(x?, ti), the funnel does not intersectthe obstacle if and only if V ?(x?, ti) ≥ ρ(ti),∀ti. Multiple obstacles are handled bysimply solving (7) for each obstacle. An important point that should be noted is thatwe do not require the obstacle regions to be convex. It is only required that they arerepresented as unions of convex polytopic sets.

For higher order polynomial Lyapunov functions, the following sums-of-squaresconditions need to be checked for all ti:

V (x, ti)−ρ(ti)−Li(x)∑j

A jx≥ 0 (8)

Li(x)≥ 0

However, these provide only sufficient conditions for non-collision. Thus, if theconditions in (8) are met, one is guaranteed that there is no intersection with theobstacle. The converse is not true in general. Further, depending on the state space


dimension of the robot, this optimization problem may be computationally expen-sive to solve online. Hence, for tasks in which online execution speed is crucial, onemay need to restrict oneself to quadratic Lyapunov functions. However, since mostpopular control design techniques like the Linear Quadratic Regulator, H∞ and LMImethods ([19] [7]) provide quadratic Lyapunov function candidates, the loss is notsubstantial.

Algorithm 1 provides a sketch of the online planning loop. At every control cy-cle, the robot updates its state in the world along with the obstacle positions. Itthen checks to see if the sequence of funnels it is currently executing may lead toa collision with an obstacle (which should only be the case if the sensors reportnew obstacles). If so, it replans a sequence of funnels that can be executed fromits current state and don’t lead to any collisions. The ReplanFunnels(x,O) subrou-tine assumes that funnel sequences that are sequentially composable in the sense ofSection 4.2 have been ordered by preference during the pre-computation stage. Forexample, sequences may be ordered by the amount they make the robot progressin its nominal direction for a navigation task. ReplanFunnels(x,O) goes throughfunnel sequences and checks two things. First, it checks that its current state is con-tained in the first funnel in the sequence (after appropriately shifting the funnel inthe cyclic dimensions). Second, it checks that the sequence leads to no collisionswith obstacles. The algorithm returns the first sequence of funnels that satisfies bothcriteria. Finally, the online planing loop computes which funnel of the current planit is in and applies the corresponding control input ui(x, t.internal).

Of course, several variations on Algorithm 1 are possible. In practice, it may notbe necessary to consider re-planning at the frequency of the control loop. Instead,longer sections of the plan may be executed before re-planning. Also, instead ofchoosing the most “preferred” collision-free sequence of funnels, one could choosethe sequence that maximizes the minimum over ti of V ?(x?,ti)

ρ(ti). As before, V ?(x?, ti)

is the solution of the QP (7). Since the 1-sublevel set of V (x,ti)ρ(ti)

corresponds to thefunnel, maximizing this is a reasonable choice for choosing sequences of funnels.

Algorithm 1 Online Planning1: Initialize current planned funnel sequence, P = {F1,F2, . . . ,Fn}2: for t = 0, . . . do3: O ⇐ Obstacles in sensor horizon4: x⇐ Current state of robot5: Collision⇐ Check if P collides with O by solving QPs (7)6: if Collision then7: P ⇐ ReplanFunnels(x,O)8: end if9: F.current⇐ Fi ∈P such that x ∈ Fi

10: t.internal⇐ Internal time of F.current11: Apply control ui(x, t.internal)12: end for


5 Example

X

Y

Fig. 2 Visualization of the system showing the coordinate system, polygonal obstacles, and “cross-wind”.

We demonstrate our approach on a model of an aircraft flying in two dimensionsthrough a forest of polygonal trees. A pictorial depiction of the model is provided inFigure 2. The aircraft is constrained to move at a fixed forward speed and can controlthe second derivative of its yaw angle. We introduce uncertainty into the model byassuming that the speed of the plane is uncertain and time-varying and that there isa time-varying “cross-wind” whose magnitude is instantaneously bounded. The fullnon-linear dynamics of the system are then given by:

x =

xyψ

ψ

, x =

−v(t)cosψ

v(t)sinψ

ψ

u

+

w(t)

000

. (9)

with the speed of the plane v(t)∈ [9.5,10.5]m/s and cross-wind w(t)∈ [−0.3,0.3m/s].The plane’s trajectory library, T , consists of 11 trajectories and is shown in Fig-

ure 3(a). The trajectories xi(t) : [0,Ti] 7→ R4 and the corresponding nominal open-loop control inputs ui(t) were obtained via the direct collocation trajectory opti-mization method [1] by constraining xi(0) and xi(Ti) and locally minimizing a costof the form:

J =∫ Ti

0[1+u0(t)T R(t)u0(t)]dt.

Here, R is a positive-definite cost matrix. For each xi(t) in T we design a H∞ con-troller that locally stabilizes the system. This is done by first computing a time-varying linearization of the dynamics and disturbances about the trajectory:


˙x≈ Ai(t)x(t)+Bi(t)u(t)+Di(t)w(t)

where x = x− xi(t) and u = u− ui(t). The optimal linear control law, u?(x, t) =−R−1Bi(t)T Si(t)x is obtained by solving the Generalized Riccati Differential Equa-tion:

−Si(t) = Q+Si(t)Ai(t)+Ai(t)T Si(t)−Si(t)[Bi(t)R−1Bi(t)T − 1γ2 Di(t)Di(t)T ]Si(t)

with final value conditions Si(T ) = ST,i. The quadratic functional:

Vi(x, t) = (x− xi(t))T Si(t)(x− xi(t))

is our locally valid Lyapunov candidate. Then, as described in Section 4.1, we useVi(x, t) to compute robust regions of finite-time invariance (“funnels”). A few imple-mentation details are worth noting here. Similar to [24], we perform the verificationon the time-varying nonlinear system by taking third-order Taylor-approximationsof the dynamics about the nominal trajectories. Secondly, the set of possible ini-tial conditions for a given funnel, X0 = {x|V (x, t) ≤ ρ(0)} needs to be specified.Since the Riccati equation does not allow us to fix S(0) directly, we can only af-fect the shape of the initial ellipsoidal region by scaling ρ(0). In order to choose aphysically meaningful set of initial conditions we specify a desired ellipsoidal setX0,des = {x|Vdes(x) ≤ 1}, and then choose the smallest ρ(0) such that X0,des ⊂ X0.The following sums-of-squares program can be solved to obtain such a ρ(0):

minimizeρ(0),τ

ρ(0) (10)

subject to ρ(0)−V (x,0)+ τ(Vdes(x)−1)≥ 0τ ≥ 0

Three of the funnels in our library are shown in Figure 3(b). Note that the funnelshave been projected down from the original four dimensional state space to the x-yplane for the sake of visualization.

Figure 5 demonstrates the use of the online planning algorithm in Section 4.3.The plane plans two funnels in advance while nominally attempting to fly in they-direction and avoiding obstacles. The projection of the full sequence of funnelsexecuted by the plane is shown in the figure. Figures 4(a) and 4(b) show the planeflying through the same forest with identical initial conditions. The only differenceis that the cross-wind term is biased in different directions. In Figure 4(a), the cross-wind is primarily blowing towards the right, while in Figure 4(b), the cross-wind isbiased towards the left. Of course, the plane is not directly aware of this difference,but ends up following different paths around the obstacles as it is buffeted aroundby the wind.

Finally, we demonstrate the utility of explicitly taking into account uncertaintyin Figure 5. There are two obstacles in front of the plane. The two options availableto the plane are to fly straight in between the obstacles or to bank right and attempt


−1.5 −1 −0.5 0 0.5 1 1.5

0

0.5

1

1.5

2

x

y

(a) Trajectory library consisting of 11 locallyoptimal trajectories.

−2 −1.5 −1 −0.5 0 0.5 1 1.5 2

0

0.5

1

1.5

2

2.5

x

y

(b) Three funnels in our funnel library pro-jected onto the x-y plane.

Fig. 3 Trajectory and funnel libraries for the plane.

0 2 4 6 8 10 12 14 16 18 200

5

10

15

20

25

30

x

y

(a) Cross-wind biased towards the right. (b) Cross-wind biased to the left.

Fig. 4 Robust online planning though a forest of polygonal obstacles. The two subfigures showthe plane flying through the same forest, but with the cross-wind biased in different directions. Theeventual path through the forest is different, as the plane goes right around the marked obstacle in(a) and left in (b).

to go around them. If the motion planner didn’t take uncertainty into account andchose the one that maximized the average distance to the obstacles, it would choosethe trajectory that banks right and goes around the obstacles. However, taking thefunnels into account leads to a different decision: going straight in between theobstacles is safer even though the distance to the obstacles is smaller.


−1 −0.5 0 0.5 1 1.5 2

0

0.5

1

1.5

2

2.5

x

y

Fig. 5 This figure shows the utility of explicitly taking uncertainty into account while planning.The intuitively more “risky” strategy of flying in between two closely spaced obstacles is guaran-teed to be safe, while the path that avoids going in between obstacles is less robust to uncertaintyand could lead to a collision.

6 Discussion and Future Work

6.1 Stochastic Verification

Throughout this paper, we have assumed that all disturbances and uncertainty arebounded with probability one. In practice, this assumption may either not be fullyvalid or could lead to over-conservative performance. In such situations, it is morenatural to provide guarantees of finite time invariance of a probabilistic nature.Recently, results from classical super-martingale theory have been combined withsums-of-squares programming in order to compute such probabilistic certificatesof finite time invariance [23]. i.e. provide upper bounds on the probability that astochastic nonlinear system will leave a given region of state space. The results pre-sented in [23] can be directly combined with the approach presented in this workto perform robust online planning on stochastic systems and will be the subject offuture work.

6.2 Continuously Parameterized Families of Funnels

As discussed in Section 4.2, we are currently partially exploiting invariances in thedynamics by shifting trajectories (and corresponding funnels) that we want to ex-ecute next in the cyclic coordinates so they line up with the cyclic coordinates ofthe robot’s current state. In our example from Section 5, this simply correspondsto translating and rotating funnels so the beginning of the next trajectory lines up


with the current state’s x,y and yaw. However, we could further exploit invariancesin the dynamics by shifting funnels around locally to ensure that they don’t inter-sect an obstacle while still maintaining the current state inside the funnel. One canthen think of the nominal trajectories and funnels being continuously parameterizedby shifts in the cyclic coordinates. Interestingly, it is also possible to use sums-of-squares programming to compute conservative funnels for cases in which oneshifts the nominal trajectory in the non-cyclic coordinates [14]. Thus, one could po-tentially significantly improve the richness of the funnel library by pre-computingcontinuously parameterized funnel libraries instead of just a finite family. However,choosing the right “shift” to apply at runtime is generally a non-convex problem(since the free-space of the robot’s environment is non-convex) and thus one canonly hope to find “shifts” that are locally optimal.

6.3 Sequence optimization for Large Funnel Libraries

For extremely large funnel libraries, it may be computationally difficult to searchall the funnels while planning online. This is a problem that traditional trajectory li-braries face too [4]. Advances in submodular sequence optimization were leveragedin [4] to address this issue. The approach involves limiting the set of trajectoriesthat are evaluated online and optimizing the sequence in which trajectories are eval-uated. Guarantees are provided on the sub-optimality of the resulting strategy. Thistechnique could be adapted to work in our framework too and will be addressed infuture work.

6.4 Designing and Evaluating Trajectory Libraries

As discussed in Section 2, the design of trajectory libraries has been the subject ofa large body of work in motion planning. One of the most exciting recent resultsis presented in [10] in which percolation theory is used to provide mathematicalbounds on the speed at which a robot can safely fly through a forest. Assumingthat the probability distribution that gave rise to the forest is ergodic and under afew technical conditions on the dynamics of the robot, it is shown that there existsa critical speed beyond which the robot cannot fly forever without collisions. Ifthe robot flies below this speed, there exists an infinite path through the forest. Webelieve that the techniques used in [10] can be used to evaluate and compare funnellibraries too. In the setting where the distribution of the obstacle generating processis known and is ergodic, we can compute the probability that there exists a sequenceof funnels that can be chained together to traverse the forest without colliding witha tree. Intuitively, the idea is to consider all possible funnel sequences through theforest and then computing the probability that there exists a particular sequence withno tree lying inside any funnel in that sequence.


This may provide an important extension to the approach presented in this papersince it allows us to directly deal with uncertainty in the nature of the environmenttoo. It also provides us with a meaningful way to evaluate whether a given funnellibrary is “good enough” or whether more primitives are required in order to have ahigh chance of success.

7 Conclusion

In this paper, we have presented an approach for motion planning in a priori un-known environments with dynamic uncertainty in the form of bounded parametricmodel uncertainty, disturbances, and state errors. The method augments the tradi-tional trajectory library approach by constructing stabilizing controllers around thenominal trajectories in a library and computing robust regions of finite time in-variance (“funnels”) for the resulting closed loop controllers via sums-of-squaresprogramming. The pre-computed set of funnels is then used to plan online by se-quentially composing them together in a manner that ensures obstacles are avoided.By explicitly taking into account uncertainty and disturbances while making motionplans, we can evaluate trajectory sequences based on how susceptible they are to dis-turbances. We have demonstrated our approach on a simulation of a plane flying intwo dimensions through a forest of polygonal obstacles. Future work will focus onextending our results to scenarios in which a stochastic description of uncertaintyis more appropriate and using the tools from this paper to evaluate different trajec-tory libraries in order to determine whether they can be successfully employed toperform the task at hand.

8 Acknowledgements

This work was partially supported by ONR MURI grant N00014-09-1-1051 and theMIT Intelligence Initiative.

References

1. J. T. Betts. Practical Methods for Optimal Control Using Nonlinear Programming. SIAMAdvances in Design and Control. Society for Industrial and Applied Mathematics, 2001.

2. R. R. Burridge, A. A. Rizzi, and D. E. Koditschek. Sequential composition of dynamicallydexterous robot behaviors. International Journal of Robotics Research, 18(6):534–555, June1999.

3. E. F. Camacho and C. Bordons. Model Predictive Control. Springer-Verlag, 2nd edition, 2004.4. Dey, D., Liu, T.Y., Sofman, B., Bagnell, and D. Efficient optimization of control libraries.

Technical report, Technical Report (CMU-RI-TR-11-20), 2011.


5. E. Frazzoli, M. Dahleh, and E. Feron. Maneuver-based motion planning for nonlinear systemswith symmetries. IEEE Transactions on Robotics, 21(6):1077–1091, December 2005.

6. C. Green and A. Kelly. Toward optimal sampling in the space of paths. In 13th InternationalSymposium of Robotics Research, November 2007.

7. M. Green and D. Limebeer. Linear Robust Control. Prentice Hall, 1995.8. Hu, TC, Kahng, A.B., Robins, and G. Optimal robust path planning in general environments.

Robotics and Automation, IEEE Transactions on, 9(6):775–784, 1993.9. Jacobs, P., Canny, and J. Robust motion planning for mobile robots. In Robotics and Automa-

tion, 1990. Proceedings., 1990 IEEE International Conference on, pages 2–7. IEEE, 1990.10. Karaman and Frazzoli. High-speed flight through an ergodic forest. In IEEE Conference on

Robotics and Automation (submitted), 2012.11. S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. Int.

Journal of Robotics Research, 30:846–894, June 2011.12. J. Kuffner and S. Lavalle. RRT-connect: An efficient approach to single-query path planning.

In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA),pages 995–1001, 2000.

13. Liu, C., Atkeson, and C.G. Standing balance control using a trajectory library. In IntelligentRobots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on, pages 3031–3036. IEEE, 2009.

14. A. Majumdar, M. Tobenkin, and R. Tedrake. Algebraic verification for parameterized motionplanning libraries. In Proceedings of the 2012 American Control Conference (ACC), 2012.

15. Mayne, D.Q., Seron, M.M., Rakovic, and SV. Robust model predictive control of constrainedlinear systems with bounded disturbances. Automatica, 41(2):219–224, 2005.

16. Mellinger, D., Kumar, and V. Minimum snap trajectory generation and control for quadrotors.In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pages 2520–2525. IEEE, 2011.

17. Y. Nesterov and A. Nemirovskii. Interior-Point Polynomial Algorithms in Convex Program-ming. Studies in Applied Mathematics. Society for Industrial Mathematics, 1995.

18. P. A. Parrilo. Structured Semidefinite Programs and Semialgebraic Geometry Methods inRobustness and Optimization. PhD thesis, California Institute of Technology, May 18 2000.

19. Petersen, I.R., Ugrinovskii, V.A., Savkin, and A.V. Robust control design using H-8 methods.Communications and control engineering series. Springer, 2000.

20. Schouwenaars, T., Mettler, B., Feron, E., How, and J.P. Robust motion planning using amaneuver automation with built-in uncertainties. In American Control Conference, 2003.Proceedings of the 2003, volume 3, pages 2211–2216. IEEE, 2003.

21. Sermanet, P., Scoffier, M., LeCun, and C.C.U.M.Y. Learning maneuver dictionaries for groundrobot planning. In Proc. 39th International Symposium on Robotics (ISR08), 2008.

22. A. Shkolnik. Sample-Based Motion Planning in High-Dimensional and Differentially-Constrained Systems. PhD thesis, MIT, February 2010.

23. J. Steinhardt and R. Tedrake. Finite-time regional verification of stochastic nonlinear systems.In Proceedings of Robotics: Science and Systems (RSS) 2011, January 17 2011.

24. R. Tedrake, I. R. Manchester, M. M. Tobenkin, and J. W. Roberts. LQR-Trees: Feedbackmotion planning via sums of squares verification. International Journal of Robotics Research,29:1038–1052, July 2010.

25. M. M. Tobenkin, I. R. Manchester, and R. Tedrake. Invariant funnels around trajectoriesusing sum-of-squares programming. Proceedings of the 18th IFAC World Congress, extendedversion available online: arXiv:1010.3013 [math.DS], 2011.

26. Topcu, U., Packard, A.K., Seiler, P., Balas, and G.J. Robust region-of-attraction estimation.IEEE Transactions on Automatic Control, 55(1):137 –142, Jan 2010.

27. Toussaint and G.J. Robust control and motion planning for nonlinear underactuated sys-tems using H-infinity Techniques. PhD thesis, PhD thesis, University of Illinois at Urbana-Champaign, Urbana, IL, 2000.

Robust Online Motion Planning with Regions of Finite Time...

Documents

Transcript of Robust Online Motion Planning with Regions of Finite Time...