Source-linked AI summary
Control Design along Trajectories with Sums of Squares Programming
Anirudha Majumdar, Amir Ali Ahmadi, Russ Tedrake
TL;DR
Challenging robotic tasks need feedback controllers with formal stability and safety guarantees, but existing approaches are limited for nonlinear trajectory tracking. The paper designs polynomial time-varying controllers and SOS Lyapunov certificates to maximize invariant funnels, with validation on a torque-limited Acrobot.
Problem
Feedback control for challenging nonlinear robotic tasks requires formal stability and safety guarantees, while existing SOS controller design has focused mainly on time-invariant systems reaching equilibria.
Method
The paper jointly designs polynomial controllers and sums-of-squares Lyapunov certificates to maximize invariant funnels leading to predefined goal sets.
Results
The method produces larger verified funnels than an LQR controller and is validated in 30 Acrobot hardware trials for swing-up and balance.
Takeaways & Limitations
The controllers can support space-filling feedback motion planning by enabling coverage with fewer planned trajectories.
Takeaways & Limitations
Modeling and state-estimation errors can violate the practical guarantees, especially when system dynamics are difficult or impossible to model accurately.
Abstract
from arXiv · showhide
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.
I. INTRODUCTION
The paper designs feedback controllers with formal stability and safety guarantees for nonlinear robotic systems tracking trajectories. It combines sums-of-squares programming with funnel maximization and validates the approach on a torque-limited Acrobot.
- I. INTRODUCTION: The paper targets feedback control methods that provide stability and safety guarantees for challenging nonlinear robotic tasks.Open-loop motion plans alone do not correct deviations from planned trajectories, while linear MPC cannot guarantee stability or safety despite handling input saturations.
- I. INTRODUCTION: The approach uses Lyapunov theory and sums-of-squares programming to turn trajectory guarantees into polynomial positivity conditions amenable to optimization.SOS methods provide machinery for computing polynomial Lyapunov functions.
- I. INTRODUCTION: The proposed method designs time-varying controllers that maximize the size of verified funnels leading states to a predefined goal set.It directly handles trajectory-induced time-varying dynamics and actuation limits, and can be extended to uncertainty.
- I. INTRODUCTION: Controllers with larger verified funnels can help space-filling feedback motion-planning algorithms cover state space with fewer planned trajectories.The intended setting is sequential composition of locally stabilizing controllers.
- I. INTRODUCTION: Extensive hardware experiments validate the method on a severely torque-limited underactuated double pendulum performing swing-up and balance.The paper identifies these experiments as the first hardware validation of SOS-programming-based funnels.
II. TIME INVARIANT CONTROLLER DESIGN
The time-invariant design searches for polynomial controllers and Lyapunov functions whose verified region of attraction is as large as possible. SOS constraints certify the region, while alternating optimization addresses bilinear decision-variable couplings.
- II. TIME INVARIANT CONTROLLER DESIGN: The method searches for polynomial state-feedback controllers that stabilize a polynomial control-affine system to an equilibrium point.Restricting controllers to fixed-degree polynomials makes the search amenable to sums-of-squares programming.
- II. TIME INVARIANT CONTROLLER DESIGN: The verified region is a Lyapunov sublevel set Bρ={x | V(x)≤ρ}, and the design objective is to maximize its size through ρ.Normalization of V is required so scaling V cannot make ρ arbitrarily large.
- II. TIME INVARIANT CONTROLLER DESIGN: SOS conditions certify Bρ as an inner approximation of the true region of attraction by enforcing positivity of V and decrease of V along closed-loop trajectories.The multiplier term L(x) enters the SOS certificate, while the normalization constraint fixes the scale of V.
- II. TIME INVARIANT CONTROLLER DESIGN: The optimization is bilinear overall but becomes linear in alternating blocks of variables, enabling iterative searches over the controller, multiplier, Lyapunov function, and ρ.The two main blocks fix V or fix the controller and multiplier; a linear controller such as LQR can initialize V.
- II. TIME INVARIANT CONTROLLER DESIGN: Removing the multiplier non-negativity constraint changes the certificate from a region of attraction to an invariant set.The boundary decrease condition then ensures trajectories starting in the set remain inside it.
III. TIME VARYING CONTROLLER DESIGN
The time-varying extension designs feedback around a nominal trajectory by expressing state and input deviations in trajectory-relative coordinates. Its objective is to maximize the finite-time funnel reaching a predefined goal set.
- III. TIME VARYING CONTROLLER DESIGN: The extension designs time-varying controllers that maximize the funnel of states stabilized to a predefined goal set along a planned trajectory.It builds on prior funnel computation for fixed feedback controllers but searches for the controller itself.
- III. TIME VARYING CONTROLLER DESIGN: Given nominal trajectory x0(t) and open-loop input u0(t), the method defines deviations x̄=x−x0(t) and ū=u−u0(t).These coordinates express tracking errors and input corrections relative to the nominal plan.
˙¯x = ˙x−˙x0(t) = f(x0(t)+ ¯x)+g(x0(t)+ ¯x)(u0(t)+ ¯u)−˙x0(t)
The funnel is represented by time-varying Lyapunov sublevel sets whose invariance is enforced at sampled times. Alternating SOS optimization jointly searches certificates, controllers, and funnel sizes.
- ˙¯x = ˙x−˙x0(t) = f(x0(t)+ ¯x)+g(x0(t)+ ¯x)(u0(t)+ ¯u)−˙x0(t): The time-varying sets Bρ(t) are designed so every state starting in the funnel reaches the goal set Bf at the terminal time.Setting Bρ(T)=Bf connects finite-time invariance to goal-set reachability.
- ˙¯x = ˙x−˙x0(t) = f(x0(t)+ ¯x)+g(x0(t)+ ¯x)(u0(t)+ ¯u)−˙x0(t): The method parameterizes V and the feedback correction with polynomials Vi(x̄) and ūi(x̄) at sampled times ti.Sampling avoids the more expensive alternative of checking polynomial inequalities over the full continuous time interval.
- ˙¯x = ˙x−˙x0(t) = f(x0(t)+ ¯x)+g(x0(t)+ ¯u)−˙x0(t): The SOS program uses Vi as Lyapunov certificates, Li as unconstrained-sign multipliers, and normalization against an initial Vguess.A piecewise-linear ρ(t) supplies ˙ρ(ti) from neighboring samples.
- ˙¯x = ˙x−˙x0(t) = f(x0(t)+ ¯x)+g(x0(t)+ ¯u)−˙x0(t): The alternating algorithm adds a controller-and-ρ step to maximize the objective ∑N_i=1 ρ(ti), and its sequence of optimal values converges.The extra step preserves explicit funnel-size optimization when the controller is not searched in the Lyapunov-function update.
A. Approach 1
The approach designs time-varying controllers and Lyapunov funnels with SOS programs, incorporating actuator saturation through piecewise Lyapunov analysis and alternating optimization.
- A. Approach 1: Actuator limits can be incorporated into the control design.Saturation is handled by checking Lyapunov conditions piecewise and adding multipliers to the SOS program.
- A. Approach 1: Algorithm 1 alternates between searching for controller and multiplier polynomials and searching for Lyapunov functions and funnel sizes.The two optimization steps fix one set of decision variables while optimizing the other, repeating until convergence.
- A. Approach 1: The method extends from single-input systems to multiple inputs, although the presented procedure is most directly described for one input.The framework is described as easily extendable to multiple inputs.
B. Approach 2
The alternative saturation formulation avoids exponential growth in SOS conditions for multi-input systems by imposing conservative per-input bounds inside the verified funnel.
- B. Approach 2: 3^m conditions for dV are generally needed to handle all combinations of input saturations.This creates exponential growth in SOS program size with the number of inputs.
- B. Approach 2: The alternative formulation replaces exponential growth with a conservative condition that keeps each applied input within its specified bounds inside the funnel.The condition adds one new constraint for every input and can be imposed in each step of Algorithm 1.
- B. Approach 2: The number of extra constraints grows linearly with the number of inputs, producing smaller optimization problems.This scaling is the computational benefit of the alternative formulation.
V. IMPLEMENTATION DETAILS
The implementation initializes the time-varying SOS design with a Lyapunov candidate derived from time-varying LQR and uses sampled funnel sizes along the trajectory.
- V. IMPLEMENTATION DETAILS: The initial Lyapunov candidate is obtained from a Riccati differential equation for the time-varying LQR controller.The linearization matrices A(t) and B(t), together with positive-definite Q and R, define the initialization.
- V. IMPLEMENTATION DETAILS: The terminal candidate V_guess(x̄,t_N)=x̄^T S_f x̄ and a choice of rho(t_N) determine the goal set.The final-value matrix satisfies S(t)=S_f.
- V. IMPLEMENTATION DETAILS: Setting rho(t_i) to a sufficiently small constant works well in practice.
B. Trajectory generation
The Acrobot evaluation combines direct-collocation trajectory generation, polynomial SOS and LQR controllers, funnel comparisons, and hardware trials under severe torque and modeling constraints.
- B. Trajectory generation: Direct collocation trajectory optimization generated the dynamically feasible open-loop swing-up plan used for the results.The plan connects the specified initial and final states; RRT and RRT* are also identified as alternatives.
- B. Trajectory generation: The Acrobot is a severely torque-limited underactuated double pendulum actuated only at its elbow joint.The friction drive imposes severe torque limitations, while the shoulder joint has no actuation.
- B. Trajectory generation: The model dynamics were Taylor expanded to degree 3 to obtain a polynomial vector field.The authors report that the expansion is not strictly necessary but produces nearly identical trajectories while avoiding trigonometric overhead.
- B. Trajectory generation: The SOS controller's verified funnel is significantly bigger than the time-varying LQR funnel along the swing-up trajectory.The verified initial-condition set for LQR is strictly contained within the corresponding SOS controller set.
- B. Trajectory generation: All 30 experimental trials successfully swung the Acrobot up and balanced it.The trials started from random initial conditions inside the SOS verified funnel and used the time-varying SOS controller before switching to cubic balancing control.
- B. Trajectory generation: Experimental trajectories stayed within the verified funnel for most of the motion but showed end-of-trajectory violations attributed to estimation and model errors.The Lyapunov function's large state gradient near the end also amplifies small Euclidean deviations; simulated trajectories remained inside.
VII. DISCUSSION
The reported cubic time-invariant and linear time-varying controllers substantially improve SOS-verified funnel sizes over LQR for the swing-up task. Higher-degree controllers and Lyapunov functions may improve results further.
- Substantial improvements in SOS-verified funnel size were obtained over LQR using cubic time-invariant and linear time-varying controllers for swing-up.
- Higher-degree controllers may produce even larger SOS-verified funnels than the reported controller degrees.
- Higher-degree Lyapunov functions may provide tighter estimates of true regions of attraction and funnels without modifying the presented approach.
A. Robustness
The method extends funnel-based control design to bounded uncertainty, obstacles, and kinematic constraints while retaining SOS-based guarantees. However, modeling and state-estimation errors can cause these guarantees to be violated in practice.
- A. Robustness: Modeling and state-estimation errors can violate the method’s guarantees in practice, although the reported hardware violations were small.The authors caution that this limitation may be more consequential when dynamics are difficult or impossible to model accurately.
- A. Robustness: Bounded polynomial disturbances can be incorporated through modified SOS programs, but funnel computation is not guaranteed for uncertain dynamics.The disturbance set must be semi-algebraic, and searching for the controller may make obtaining a funnel more likely.
- A. Robustness: Polytopic obstacles and kinematic constraints such as joint limits can be incorporated by enforcing conditions that keep the funnel outside obstacles.The stated condition uses half-plane constraints and ensures that points inside an obstacle lie outside the computed funnel.
- A. Robustness: The overall procedure uses Lyapunov inequalities and sums-of-squares programming to optimize guaranteed funnels for time-varying controllers under input constraints.The funnel represents initial conditions whose trajectories are driven to the goal set.