Control Design along Trajectories with Sums of Squares Programming

Anirudha Majumdar, Amir Ali Ahmadi, Russ Tedrake

Abstract

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.

Introduction

In robotics, generating an open-loop trajectory is only half the challenge. To handle real-world disturbances, a robot requires a feedback controller to remain on that path. However, standard feedback techniques often lack formal stability guarantees. For instance, the Linear Quadratic Regulator (LQR) relies on linearization and cannot guarantee stability for the original nonlinear system, particularly near actuator limits. Model Predictive Control (MPC) handles constraints well but rarely provides explicit safety certificates for the entire state space. While Dynamic Programming offers guarantees, it is computationally prohibitive for high-dimensional systems.

This work addresses the need for formal guarantees by identifying the set of initial conditions that will successfully reach a goal under limited torque and complex nonlinear dynamics.

Methodology

The proposed method utilizes Lyapunov Stability Theory combined with Sums of Squares (SOS) Programming to design robust feedback controllers.

Lyapunov Funnels and SOS

A Lyapunov function acts as a scalar "energy" metric; if it decreases along a trajectory, the system is stable. The "funnel" represents the region in state space where this stability proof holds. Since verifying positive definiteness for general polynomials is NP-hard, we employ SOS programming to relax the problem into a semidefinite program (SDP), which can be solved efficiently.

Controller Synthesis

Unlike previous methods that only verified existing controllers, our approach simultaneously designs the controller ($u$) and the Lyapunov function ($V$) to maximize the funnel's volume. Because this joint optimization is non-convex, we use a bilinear alternation strategy: iteratively fixing the controller to optimize the Lyapunov function, and vice-versa. Crucially, actuator limits (e.g., torque saturation) are explicitly incorporated into the SOS constraints, ensuring the stability proof accounts for physical limitations.

Experimental Validation

The algorithm was evaluated on an Acrobot—a double pendulum with only one motor at the elbow, making it a classic underactuated control challenge.

The underactuated Acrobot hardware platform used for experimental validation.

Validation included 100 simulated trials and 30 physical trials on custom hardware. The hardware experiments were designed to test the "swing-up and balance" maneuver from various initial conditions within the calculated SOS funnel.

Results and Discussion

The SOS-designed cubic controller demonstrated significantly larger basins of attraction compared to standard time-invariant LQR controllers.

Comparison of time-varying invariant funnels for SOS and LQR controllers along a swing-up trajectory. The SOS approach (red) provides a much larger guaranteed stable region than LQR (blue).

In hardware trials, the Acrobot achieved a 100% success rate (30/30) when starting within the SOS funnel. This represents the first hardware validation of SOS-programming-based funnels in robotics. While real-world factors like sensor noise and friction slippage caused minor deviations, the system remained robust enough to succeed in every trial.

Limitations and Future Work

Despite its success, SOS programming scales poorly with the number of state variables, making application to high-DOF robots (like humanoids) challenging. Additionally, the iterative optimization can converge to local optima. Future work will focus on robust formulations to handle model uncertainty and the integration of kinematic constraints directly into the funnel optimization process.

Conclusion

By shifting the focus from verification to design, we have developed a method to maximize safety margins for nonlinear robotic systems. The resulting "funnels" allow motion planning algorithms to cover the state space more efficiently with fewer trajectories, backed by formal stability proofs.

Summary updated

Abstract

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.
BibTeX
@inproceedings{majumdar2013control,
  author = {Anirudha Majumdar and Amir Ali Ahmadi and Russ Tedrake},
  title = {{Control Design along Trajectories with Sums of Squares Programming}},
  booktitle = {IEEE International Conference on Robotics and Automation},
  year = {2013},
  url = {https://openreview.net/forum?id=7aQx37tw3dB},
}
Figure