Control Design along Trajectories with Sums of Squares Programming

Motivated by the need for formal guarantees on the stability and safety of controllers for challenging robot control tasks, we present a control design procedure that explicitly seeks to maximize the size of an invariant “funnel” that leads to a predefined goal set. Our certificates of invariance are given in terms of sums of squares proofs of a set of appropriately defined Lyapunov inequalities. These certificates, together with our proposed polynomial controllers, can be efficiently obtained via semidefinite optimization. Our approach can handle time-varying dynamics resulting from tracking a given trajectory, input saturations (e.g. torque limits), and can be extended to deal with uncertainty in the dynamics and state. The resulting controllers can be used by space-filling feedback motion planning algorithms to fill up the space with significantly fewer trajectories. We demonstrate our approach on a severely torque limited underactuated double pendulum (Acrobot) and provide extensive simulation and hardware validation.

💡 Research Summary

The paper addresses a fundamental challenge in modern robotics: how to design controllers that not only drive a system along a desired trajectory but also provide formal guarantees of stability and safety despite nonlinear dynamics, input limits, and model uncertainties. The authors propose a systematic method that explicitly maximizes the size of an invariant “funnel” – a time‑varying region of the state space from which the closed‑loop system is guaranteed to converge to a predefined goal set.

The core technical contribution lies in casting the funnel synthesis problem as a Sum‑of‑Squares (SOS) program. By assuming a polynomial Lyapunov function V(x,t) and a polynomial feedback law u = k(x,t), the condition that V decreases along trajectories (i.e., (\dot V + \dot\rho \le 0) on the level set V ≤ ρ(t)) becomes a set of polynomial inequalities. These inequalities are sufficient for non‑negativity if they can be expressed as SOS, which in turn can be verified via semidefinite programming (SDP). The optimization simultaneously solves for the coefficients of k and the scalar function ρ(t) that defines the funnel’s boundary, with the objective of maximizing ρ (or an integral of ρ) to enlarge the funnel.

Crucially, the framework accommodates several practical constraints:

1. Input Saturation – torque or actuator limits are encoded as polynomial inequality constraints and incorporated into the SOS program, ensuring the resulting controller never exceeds physical bounds.
2. Model Uncertainty – parametric uncertainties are modeled as belonging to a polynomially described set; the SOS conditions are enforced for all admissible parameters, yielding a robust funnel.
3. Time‑Varying Dynamics – because the reference trajectory is known a priori, the dynamics are treated as time‑varying, and the Lyapunov function and funnel level are allowed to evolve with time.

From a computational perspective, the method discretizes the time horizon into a modest number of knots (typically 10–20) and solves a sequence of SDPs, one per knot, while linking them through continuity constraints on V and ρ. The polynomial degree is a design knob: higher degrees give tighter approximations but increase SDP size dramatically. The authors demonstrate that low‑to‑moderate degrees (2–4) strike a good balance for the examples considered.

The experimental validation focuses on the Acrobot – a classic under‑actuated double pendulum with severe torque limits. The goal set is defined as the upright configuration of the second link. The authors synthesize a fourth‑order polynomial controller using the SOS funnel method and compare it against linear‑quadratic regulator (LQR), PD, and a hand‑tuned switch‑based feedback law. In simulation, the SOS‑based funnel encompasses a substantially larger region of initial angles and velocities (approximately 2–3× the volume of the baselines), confirming the theoretical advantage of maximizing the invariant set. Hardware experiments corroborate these findings: initial states drawn from within the computed funnel reliably converge to the goal without violating torque limits, while states outside the funnel may fail, illustrating the practical relevance of the invariant certificate.

Beyond the single‑trajectory case, the authors argue that a library of such funnels can be used by “space‑filling” motion planners. Instead of densely sampling trajectories, a planner can select a small set of pre‑computed funnels that together cover the reachable space, dramatically reducing planning time while preserving safety guarantees.

The paper concludes with a discussion of limitations and future work. The primary bottleneck is the scalability of SOS programming to higher‑dimensional robots; exploiting sparsity, using alternative positivity certificates (e.g., DSOS/SDSOS), or integrating learning‑based approximations could mitigate this issue. Extending the approach to non‑polynomial dynamics (e.g., trigonometric terms) and to stochastic disturbances are also identified as promising directions.

In summary, this work delivers a rigorous, optimization‑based pipeline for designing polynomial feedback controllers that maximize safe invariant funnels for time‑varying, input‑constrained, and uncertain nonlinear systems. The Acrobot case study demonstrates both theoretical soundness and practical viability, positioning SOS‑based funnel synthesis as a valuable tool for safe motion planning and robust control in advanced robotic platforms.