Source-linked AI summary
Funnel Libraries for Real-Time Robust Feedback Motion Planning
Anirudha Majumdar, Russ Tedrake
TL;DR
The paper addresses how to generate provably safe real-time motion plans for nonlinear robots facing uncertainty and environments revealed only at runtime. It pre-computes SOS-based robust funnels and composes them online, demonstrating the approach in simulations and fixed-wing hardware experiments at approximately 12 miles per hour.
Problem
Agile robots lack safety guarantees when nonlinear dynamics, disturbances, modeling uncertainty, and runtime-unknown obstacle geometry must be handled during real-time planning.
Method
The method builds an offline library of feedback-controlled funnels using SOS programming to bound and minimize uncertain reachable sets, then sequentially composes these plans at runtime.
Results
The approach was demonstrated in ground-vehicle and quadrotor simulations and in fixed-wing hardware experiments avoiding cluttered obstacles at approximately 12 miles per hour.
Takeaways & Limitations
Explicitly modeling uncertainty lets the robot compare maneuver vulnerability and choose a more robust primitive or abort when no maneuver is guaranteed to succeed.
Takeaways & Limitations
Time-sampled funnel computation can provide significant computational gains but does not provide an exact funnel certificate.
Abstract
from arXiv · showhide
We consider the problem of generating motion plans for a robot that are guaranteed to succeed despite uncertainty in the environment, parametric model uncertainty, and disturbances. Furthermore, we consider scenarios where these plans must be generated in real-time, because constraints such as obstacles in the environment may not be known until they are perceived (with a noisy sensor) at runtime. Our approach is to pre-compute a library of "funnels" along different maneuvers of the system that the state is guaranteed to remain within (despite bounded disturbances) when the feedback controller corresponding to the maneuver is executed. We leverage powerful computational machinery from convex optimization (sums-of-squares programming in particular) to compute these funnels. The resulting funnel library is then used to sequentially compose motion plans at runtime while ensuring the safety of the robot. A major advantage of the work presented here is that by explicitly taking into account the effect of uncertainty, the robot can evaluate motion plans based on how vulnerable they are to disturbances. We demonstrate and validate our method using extensive hardware experiments on a small fixed-wing airplane avoiding obstacles at high speed (~12 mph), along with thorough simulation experiments of ground vehicle and quadrotor models navigating through cluttered environments. To our knowledge, these demonstrations constitute one of the first examples of provably safe and robust control for robotic systems with complex nonlinear dynamics that need to plan in real-time in environments with complex geometric constraints.
1 Introduction
The paper targets real-time motion planning for agile robots whose nonlinear dynamics, uncertainty, and runtime-unknown environments undermine safety guarantees. It combines offline robust funnel computation with runtime sequential composition and validates the approach in simulation and hardware.
- Motivation: Agile robots operating near obstacles can fail dramatically because their safety is not guaranteed under environmental and dynamical uncertainty.Examples include high-speed UAVs, legged robots, and mobile manipulators.
- Challenges: The planning problem combines nonlinear underactuated dynamics, input constraints, disturbances and modeling error, and geometry revealed only at runtime.These conditions force the robot to plan in real time while accounting for uncertainty.
- Approach: The approach pre-computes motion trajectories and SOS-based feedback funnels that bound worst-case reachable states under uncertainty, then composes them at runtime.The funnel library supports robust planning in previously unseen environments.
- Uncertainty-aware decisions: Funnel selection accounts for how maneuvers respond to disturbances, so a less aggressive maneuver or mission abort can be preferred over a dynamically vulnerable maneuver.The paper illustrates this trade-off using UAV roll and straight-flight primitives under wind gusts and initial-state perturbations.
- Evaluation: The method is evaluated in simulations of ground vehicles and quadrotors and in hardware experiments with a fixed-wing airplane avoiding obstacles at approximately 12 miles per hour.The experiments use cluttered or previously unseen environments.
2 Relevant Work
The paper builds on trajectory libraries, robust planning, reachability, Lyapunov funnels, and SOS methods while targeting nonlinear systems and runtime-unknown environments. Its distinctions include real-time planning, nonlinear SOS reachability computation, and explicit tube-size minimization.
- Trajectory libraries: Trajectory libraries and Maneuver Automata provide prior frameworks for representing motion primitives and composing them for real-time planning.These ideas have been applied to humanoid balance, ground vehicles, grasping, and UAV navigation.
- Planning under uncertainty: Chance-constrained, belief-space, and POMDP approaches model uncertainty in different ways, but belief-space methods generally lack robustness guarantees and POMDPs typically discretize state and action spaces.The paper focuses on continuous-state and continuous-action dynamical systems.
- Reachability analysis: Reachability methods compute forward or backward reachable-set approximations, while this paper computes nonlinear-system funnels without relying on system linearization.The authors state that this can be less conservative for highly nonlinear systems and extends beyond verification.
- Related robust planning: Compared with Ny and Pappas, the paper emphasizes real-time planning with runtime-unknown obstacles and uses SOS programming to compute and explicitly minimize tube size.The related approach assumes obstacle positions are known a priori.
- Related robust planning: Tube-based model predictive control and flow-tube methods are closely related, but the cited approaches are generally limited to linear systems.Barrier-certificate methods address obstacle avoidance but assume obstacle locations and geometry are known.
- Funnels and SOS: Lyapunov-function funnels and SOS programming provide the computational basis for certifying nonlinear feedback motion primitives.Earlier funnel methods lacked automatic Lyapunov-function search, while LQR-Trees cannot handle tasks and environments unknown until runtime.
3 Background
The background introduces semidefinite programming and sums-of-squares programming as convex tools for certifying polynomial nonnegativity, including on constrained semialgebraic sets. These certificates become optimization constraints through positive-semidefinite matrix representations and multiplier polynomials.
- Semidefinite programming: A semidefinite program optimizes over symmetric positive-semidefinite matrices subject to linear and positive-semidefiniteness constraints.Its objective is linear in the decision matrix, and SDPs include LP, QP, and SOCP as special cases.
- SOS programming: SOS programming provides a computationally tractable sufficient condition for polynomial nonnegativity, which is NP-hard to decide in general.An SOS polynomial admits a positive-semidefinite Gram-matrix representation p(x) = v(x)^T Qv(x).
- SOS programming: The vector v(x) contains all monomials up to half the degree of p, while coefficient matching imposes linear constraints on the Gram matrix Q.This converts SOS certification into semidefinite programming.
- Constrained nonnegativity: SOS constraints can certify that a polynomial is nonnegative on a basic semialgebraic set defined by polynomial equalities and inequalities.Multiplier polynomials enforce the constraints through terms associated with the set's defining polynomials.
- Notation: The paper distinguishes polynomial indeterminates from optimization decision variables, which are typically polynomial coefficients.This distinction clarifies the variables being certified versus optimized.
4 Computing Funnels
The paper computes feedback funnels as tight outer approximations of reachable sets for nonlinear systems, using SOS programming to enforce invariance and optimize funnel size under uncertainty.
- Funnel formulation: Funnels associate each time t with a set F(t) that outer-approximates states reachable from an initial-condition set under closed-loop dynamics.A funnel is represented through sublevel sets of a nonnegative time-varying function V and boundary function ρ.
- Funnel formulation: The sufficient invariance condition requires V to grow more slowly than ρ on the funnel boundary, keeping boundary states inside the funnel.This condition is imposed for every t in [0,T].
- Optimization objective: Funnel computation minimizes reachable-set volume, unlike prior fixed-goal formulations that maximize the initial set stabilized to the goal.The minimization is intended to bound uncertainty effects for collision avoidance during real-time planning in unseen environments.
- SOS formulation: SOS programming converts polynomial funnel conditions into finite-dimensional semidefinite programs, using multiplier polynomials to certify nonnegativity.The initial-condition and boundary constraints are expressed as SOS conditions when dynamics and V, ρ are polynomial.
- Volume optimization: For quadratic V, funnel cross-sections are ellipsoids, so SDP minimizes volume by maximizing det(S_k).For general V, ellipsoids provide an upper bound on the sampled funnel-volume cost.
- Numerical solution: Alternating optimization updates controller, Lyapunov, funnel-boundary, and multiplier variables while decreasing the cost monotonically, though global optimality is not guaranteed.An additional controller-and-boundary update can improve the quality of the resulting controllers and funnels.
A. Approach 1
The first extension verifies funnels with actuator saturation by analyzing the Lyapunov condition separately across saturated and unsaturated input regimes.
- Actuator saturation: The verification framework incorporates actuator limits and extends from the examined single-input case to multiple inputs.The control input is mapped through a saturation function with lower and upper bounds.
- Algorithm 2: Algorithm 2 alternates three volume-minimization steps over the controller, boundary function, Lyapunov function, multipliers, and ellipsoid matrices.Each step fixes one variable block while optimizing the others.
- SOS implementation: Additional multipliers enforce the saturation-dependent Lyapunov conditions within the SOS optimization program or its time-sampled counterpart.The conditions distinguish inputs below the minimum, above the maximum, and between the limits.
B. Approach 2
The second extension addresses many-input systems, projected funnel objectives, and practical initialization, trading computational complexity or exactness against tractability and solution quality.
- Many-input systems: For m inputs, direct saturation analysis generally requires 3^m derivative conditions, causing exponential SOS-program growth.An alternative formulation adds conservativeness while avoiding this exponential increase.
- Many-input systems: A conservative alternative constrains the applied input to remain within bounds throughout the funnel, adding one condition per input and therefore linear constraint growth.This condition is explicitly identified as conservative.
- Projected funnels: Projected-volume minimization uses a projection matrix P and linearizes det(S_k^(p)) at the previous iterate because the exact objective is nonlinear in S_k.The linearization is optimized within each iteration.
- Implementation: Direct collocation generates dynamically feasible open-loop trajectories and corresponding nominal states used in the reported results.The paper notes that other trajectory-generation methods can also be used.
- Initialization: LQR-based Lyapunov candidates and an increasing ρ function initialize the alternating algorithms, but initialization can affect solution quality because global optimality is not guaranteed.Multiple possibly random initializations are suggested to address local solutions.
5.1 Sequential composition
Funnels can be composed into robust motion plans when one funnel’s terminal outlet lies inside another funnel’s initial inlet.
- Sequential composition: Sequential composition preserves the funnel property, allowing individual robust motion primitives to form composite plans.The containment condition is described as outlet-to-inlet compatibility.
- Sequential composition: A funnel pair is sequentially composable when F1(T1) ⊂ F2(0).The first funnel’s terminal set must be contained in the second funnel’s initial set.
5.2 Exploiting invariances in the dynamics
The paper exploits dynamical invariances by shifting funnels along cyclic coordinates, allowing funnels that are otherwise incompatible to become sequentially composable.
- 5.2 Exploiting invariances in the dynamics: Cyclic coordinates partition the state into components that do not affect the dynamics, enabling state transformations that preserve valid system solutions.For quadrotors and fixed-wing airplanes, suitable coordinates include position and yaw variables.
- 5.2 Exploiting invariances in the dynamics: A funnel shifted along invariant cyclic coordinates remains a valid funnel when its feedback controller is transformed correspondingly.The dynamics therefore generate an infinite family of shifted funnels from one funnel.
- 5.2 Exploiting invariances in the dynamics: Sequential composition modulo invariances permits shifting F2 so that the outlet of F1 lies within the shifted funnel’s inlet.This generalizes ordinary composition, which requires F1(T1) ⊂ F2(0).
- 5.2 Exploiting invariances in the dynamics: Compatibility of nominal trajectories is neither necessary nor sufficient for compatibility of the funnels themselves under this generalized notion.The criterion concerns containment between funnel sets rather than only their underlying nominal trajectories.
5.3 Runtime composability
Runtime composability guarantees that, after any state reached at the end of one funnel, the next funnel can be shifted at runtime to contain that state.
- 5.3 Runtime composability: Runtime composability exploits system invariances during execution, extending the offline composition relationships used to construct funnels.The paper distinguishes runtime use from offline sequential composition.
- 5.3 Runtime composability: Runtime composability requires that every state in F1(T1) can be included in F2(0) after some shift along cyclic coordinates.The required shift may depend on the observed outlet state and therefore is selected at runtime.
- 5.3 Runtime composability: Runtime shifting allows a robust motion plan computed for one starting position to be reused when the UAV’s current state differs along invariant coordinates.The funnel is shifted so its inlet contains the current state.
- 5.3 Runtime composability: Projection containment of the non-cyclic outlet in the non-cyclic inlet can certify runtime composability even when full MI composability fails.Figure 4 gives the condition πnc(F1(T1)) ⊂ πnc(F2(0)).
5.4 Checking composability
The paper checks funnel composability through containment tests, using SOS programming generally and semidefinite programming for ellipsoidal endpoint sets and projections.
- 5.4 Checking composability: The composability checks operate on polynomially represented funnel endpoint sets and are performed during offline preprocessing.The section covers sequential, MI, and runtime composability.
- 5.4 Checking composability: Sequential composability is checked with an SOS program derived from the generalized S-procedure.The test verifies the required endpoint containment condition.
- 5.4 Checking composability: For ellipsoidal endpoints, searching for the cyclic shift that establishes MI composability is formulated as an SDP over the shift and a scalar multiplier.The shifted funnel is represented using the ellipsoidal endpoint description.
- 5.4 Checking composability: The SDP feasibility condition is equivalent to MI composability for the ellipsoidal case.The matrix semidefiniteness constraint is linear in the decision variables.
- 5.4 Checking composability: Runtime composability for ellipsoidal endpoints is checked by projecting both ellipsoids onto non-cyclic coordinates and testing containment with SDP.The projection of an ellipsoid onto a linear subspace remains an ellipsoid.
- 5.4 Checking composability: For non-ellipsoidal runtime checks, projection introduces quantified conditions that make SOS formulations challenging, so the examples use ellipsoidal inlets and outlets.General-purpose quantifier-elimination tools exist, but their worst-case complexity is poor.
5.5 Funnel library
A funnel library packages funnels, runtime-composability relationships, feedback controllers, and execution times into a data structure for robust real-time planning.
- 5.5 Funnel library: A funnel library consists of a funnel set, a directed runtime-composability graph, associated feedback controllers, and execution times.These components are represented as FL = (F, G(F), C, {τi}).
- 5.5 Funnel library: The library graph records which funnels can be executed after which others at runtime.Its edges represent runtime composability relationships.
- 5.5 Funnel library: Funnels in the library may themselves be constructed by offline sequential composition, including composition modulo invariances.The library definition explicitly references runtime composability while implicitly exploiting offline MI composition.
- 5.5 Funnel library: For continuous-operation tasks, strong connectivity of the graph may be useful, whereas the paper does not impose graph connectedness or pursue task-specific graph properties.Graph diameter and girth are identified as possible factors in task efficiency.
6 Real-time Planning with Funnels
At runtime, the planner selects and shifts a library funnel so it contains the current state and avoids perceived obstacles. It replans receding-horizon style, using nonlinear or convex optimization depending on the available structure, with a verified failsafe when no funnel is found.
- 6.1 Shifting funnels at runtime: The planner searches library funnels that are composable with the previously executed funnel and selects one whose shifted version contains the current state and avoids obstacles.The shift is chosen along cyclic coordinates, and the shifted funnel must be collision-free with respect to the sensed obstacle set.
- 6.1 Shifting funnels at runtime: A basic shift aligns the funnel’s nominal trajectory with the robot’s current state in cyclic coordinates, after which standard collision-checking can test the shifted funnel.The funnel is projected onto configuration space for obstacle intersection checks.
- 6.2 Global Planning: The receding-horizon loop replans when a funnel finishes, the state leaves it, or it is no longer collision-free with newly sensed obstacles.Sensors provide state estimates and obstacle geometry within a finite horizon; a failsafe maneuver is assumed if collision-free funnel search fails.
- 6.1 Shifting funnels at runtime: The shift-search problem is generally non-convex, but it has few decision variables because only cyclic-coordinate displacements are optimized.The shift is parameterized by a vector whose non-cyclic components are fixed to zero.
- 6.1 Shifting funnels at runtime: For UAV-like systems, ellipsoidal inlet containment and convex obstacle representations yield convex quadratic constraints for efficient shift optimization.Non-ellipsoidal inlets can receive an ellipsoidal inner approximation computed offline, while non-convex obstacles are decomposed into convex segments.
- 6.2 Global Planning: Grid-based planners can search for collision-free funnel sequences, while funnel-library methods avoid computing new funnels online and account for runtime obstacle collisions.The paper contrasts this with trajectory-library and LQR-Trees-style approaches that do not incorporate unknown runtime constraints in offline computation.
7 Examples
The examples apply funnel-library planning to ground vehicles and quadrotors, combining offline funnel computation with real-time obstacle avoidance. Ground-vehicle simulations compare funnel-based planning against trajectory-based planning, while quadrotor analysis derives geometric conditions for collision-free funnel selection.
- 7.1 Ground Vehicle Model: The ground-vehicle trajectory library contains 20 trajectories generated by direct collocation, with varied final x positions and fixed initial state.The nominal speed is 10 m/s, and the nominal control input is constrained to [−500, 500] rad/s2.
- 7.1 Ground Vehicle Model: Each ground-vehicle trajectory receives a feedback controller and funnel computed using polynomial dynamics, uncertainty handling, input saturation constraints, and LQR initialization.A degree-3 time-varying Taylor expansion and 15 time samples are used; each funnel requires approximately 5–10 minutes to compute.
- 7.1 Ground Vehicle Model: The resulting ground-vehicle funnel library is used for real-time planning through randomly placed obstacles, with executed funnels illustrated in Figure 8.The environment uses randomly generated obstacles, side barriers, and a sensor horizon of 3 m forward and ±2 m laterally.
- 7.1 Ground Vehicle Model: 50 Hz is the approximate worst-case runtime of the ground-vehicle planner when all library funnels must be checked for collisions.The comparison evaluates funnel-library planning against a trajectory-library method that selects maneuvers by nominal Euclidean clearance and then applies time-varying LQR feedback.
- 7.2 Quadrotor Model: For quadrotor planning, projecting 2.5D environments and funnels onto the x−y plane supports geometric conditions intended to guarantee a collision-free funnel.The analysis relates obstacle positions, separations, sizes, and funnel reachability, and assumes the quadrotor state lies within every funnel inlet.
8 Hardware experiments on a fixed-wing airplane
Hardware experiments tested whether an identified fixed-wing airplane model could support valid funnels and real-time obstacle avoidance. The airplane remained inside a validated funnel across 30 trials and successfully negotiated 14 of 15 environments, with one failure involving a more aggressive maneuver.
- Hardware platform: The experiments used a lightweight, highly maneuverable SBach RC airplane with four control inputs sent at 50 Hz.The platform weighed 76.6 g and used ailerons, rudder, elevator, and throttle commands.
- Hardware platform: Obstacle geometry was revealed only after launch, requiring the planner to operate in a receding-horizon setting.The airplane traversed the room while avoiding obstacles whose positions and geometry were reported after clearing the launcher.
- Modeling and system identification: The airplane’s 12-state nonlinear model used measured and experimentally fitted parameters for aerodynamic and inertial effects.Parameters included moments of inertia and body drag, with additional aerodynamic corrections fit from flight data.
- Funnel validation: All 30 experimental trajectories remained inside the computed 12-dimensional funnel for the full maneuver.The normalized Lyapunov function stayed within its 1-level set, suggesting the model produced funnels valid for the hardware system.
- Obstacle avoidance experiments: 14 of 15 obstacle environments were successfully negotiated, while the single failure involved an aggressive maneuver whose controller significantly saturated the inputs.Repeated flights on one environment were all successful, supporting robustness to initial conditions.
- Obstacle avoidance experiments: Shifting funnels with the QCQP-based algorithm was crucial for finding collision-free plans in challenging environments.The comparison showed collision with obstacles without shifting and a collision-free funnel when shifting was applied.
9 Discussion and Conclusion
The paper combines stabilizing feedback controllers, SOS-computed funnels, and online composition to provide robust real-time planning under uncertainty. Experiments demonstrate safety and robustness across simulated and hardware systems, while the discussion identifies computational, geometric, library-design, sensing, and uncertainty-bound limitations.
- Approach: The method constructs stabilizing controllers around nominal trajectories and SOS-computed outer reachable-set approximations, then composes funnels online to avoid obstacles.The library addresses bounded parametric model uncertainty and external disturbances in previously unknown environments.
- Results: Ground-vehicle simulations show significant advantages over a trajectory-based approach, while quadrotor experiments establish collision-free flight for certain environment classes.The paper also reports extensive fixed-wing hardware validation in previously unseen cluttered environments.
- Results: Hardware experiments show that robust behavior need not be conservative: the fixed-wing airplane performs aggressive maneuvers while remaining robust.The experiments used a small fixed-wing airplane flying through cluttered environments at high speed.
- Limitations: Semidefinite-programming solvers remain relatively immature, causing numerical scaling issues and requiring ad hoc remedies such as rescaling data or removing redundant variables.The paper notes that newer preprocessing methods and solver technology may improve this limitation.
- Limitations: Complex robot geometries make collision checking difficult because computing obstacle representations in configuration space is challenging for non-trivial geometries.The reported examples avoid much of this difficulty by approximating robot geometry with spheres and inflating spatial funnel projections.
- Limitations: Larger funnel libraries increase the computational burden of online search, creating a tradeoff between library richness and real-time planning cost.The discussion suggests ordering or predicting collision checks to accelerate search.