Funnels, time-varying invariant sets around nominal trajectories, certify that a closed-loop system tracks its plan, and can be composed into verified motion plans. Their computation by sum-of-squares (SOS) programming is well established in formulation but not in practice: the result depends on implementation choices such as the initial guess, Lyapunov function normalization, and termination rule, which the literature documents only briefly. This paper presents a self-contained treatment of SOS-based funnel computation, implemented in the open-source package computeSOSFunnels. We propose a minimum-volume formulation with inlet and outlet constraints under which funnels with coinciding nominal endpoints compose by construction. Numerical experiments on a unicycle, with timing studies on a cart-pole and a 12-state quadrotor, characterize the pipeline: infeasible initial guesses are partly decidable before solving, the certificate shows no violations among 39,000 boundary samples per funnel under the true dynamics, and constructing the semidefinite program, not solving it, dominates computation time.
Figures & tables
Fig. 1: Funnel computation pipeline. The TVLQR cost-to-go matrices Sk warm-start the bilinear alternation of SOS program solve.
Parameter
Value
Dynamics
x˙=vcosθ,y˙=vsinθ,θ˙=ω
xi , xf
[00π/2]⊤ , [24π/2]⊤
Ufeas
∣v∣≤ 0.75 m/s, ∣ω∣≤π /2 rad/s
ΔTmax , K+1
0.25 s, 40
Rnom , wt
diag([0.1, 0.1]), 1
Q , R
diag([1, 1, 0.1]), diag([1, 0.5])
TABLE I: Unicycle problem and Solver parameters
Fig. 2: Funnel computed by Algorithm 1 for a unicycle system, terminated at iter=4 . (a) Projections of the cross-sections F(tk) onto xy -plane, with the inlet set EI and the outlet set ET . (b) Projection onto θ -space over time; vertical bars mark the θ -extent of EI and ET . (c) Funnel cross-sections in (x,y,θ) .
ρg↓\c→
1
1.5
2
3
0.02
C
C
0.152 ∣ 2355
–
0.035
C
0.159 ∣ 2407
0.153 ∣ 2779
–
0.05
0.155 ∣ 2205
0.157 ∣ 2400
–
–
0.0625
C
C
C
C
TABLE II: Sweep over the initial guess hyperparameters (ρg,c) with fixed EI,ET , and itermax=4 . Each cell entry: funnel volume ( 17 ) ∣ inlet-to-outlet volume ratio vol(B0)/vol(BK) . ‘–’: the initial (V,ρ) guess fails, i.e., L-step (line 3 , Algorithm 1 ) is infeasible. ‘C’: the initial guess violates inlet or outlet containment.
System
n
K+1
degλk
Gram size
PSD entries
L-step [s]
V-step [s]
Unicycle
3
40
2
10
2401 / 2446
3.11 (0.029)
4.73 (0.078)
Unicycle
3
40
4
20
8446 / 8491
6.43 (0.078)
9.11 (0.21)
Unicycle
3
40
6
35
24826 / 24871
13.3 (0.26)
17.7 (0.61)
Cart-pole
4
25
2
15
3152 / 3186
2.87 (0.039)
4.19 (0.11)
Cart-pole
4
25
4
35
15392 / 15426
8.72 (0.26)
12.1 (0.6)
Cart-pole
4
25
6
70
59912 / 59946
29.5 (0.96)
36.3 (2.3)
TABLE III: Cost per alternation iteration of Algorithm 1 , averaged over three iterations. Gram size g=(dn+d) , with d given by ( 20 ). PSD entries: scalarized entries of all psd blocks as formulated (L-step / V-step). Times: wall-clock seconds, with MOSEK solver time in parentheses. † Analytic count (the run did not complete on the test machine).
Fig. 3: Objective Jiter and funnel volume ( 17 ) over 20 iterations of Algorithm 1 on the default problem, each normalized by its value at the initial guess ( iter=0 ). The dotted line at iter=4 marks termination under convergence tolerance δ=0.05 . The objective J is monotone by construction (the funnel volume need not be).
Semidefinite programming optimizes linear objectives over positive semidefinite matrices to solve convex problems and build tractable relaxations and certificates.
Safety-critical control of autonomous systems requires formal safety certificates, such as controlled invariant sets, that must be computed online as conditions change. Although standard synthesis algorithms scale poorly with state dimension, monotone dynamical systems with lower-closed safety specifications allow for accelerated computation of controlled invariant sets. In particular, lazy fixed-point algorithms exploit monotonicity and track only the antichain basis of the set. However, membership tests and redundancy checks against an evolving basis remain major bottlenecks. We introduce a threshold-function reformulation in which a lower-closed set on a d-dimensional grid is represented by its column heights along a designated axis. This reformulates the greatest-fixed-point iteration as independent one-dimensional binary searches, one per grid column, yielding an embarrassingly parallel iteration with asymptotically lower computational complexity than the lazy fixed-point algorithm. Experiments synthesize invariant sets on 3D grids with 10^9 cells in under 50 ms and 10^14 cells in under two minutes. We further demonstrate online re-synthesis in a safety-informed model predictive controller example.
Yasin Sonmez, Mahmoud Khaled, Majid Zamani +1
University of California, Berkeley, USA · Ludwig-Maximilian University, Germany · University of Colorado Boulder, USA
Dynamical Systems (DS) are reactive motion policies representing vector fields trained with theoretical guarantees of stability and convergence. To ensure safety during deployment in unknown environments they must be locally reshaped, either through modulation or geometric control barrier function strategies. However, depending on the geometry of the obstacles and the complexity of the DS, these local strategies can lead the system to unavoidable collisions or spurious attractors. In this work, we certify safety with a value function drawn from the notion of backward reachability tube, which measures the worst-case safety along a rollout trajectory of the nominal DS. Usually, such a value function is intractable for a controlled system due to curse of dimensionality. We show that in the DS-based learning-from-demonstration setting, the absence of a control input collapses the reachability problem to a deterministic rollout, and the presence of certain stability conditions truncates the infinite horizon to a finite one, resulting in a well-defined value function. We further show that the value function we devised is the maximal forward-invariant subset of the obstaclefree region for the nominal DS flow. The application of this certificate function is validated across five DS constructions - analytical, Neural ODE, diffeomorphic latent space, LPV-DS, SE(3)and validate it on a Franka manipulator. Modulation and geometric CBFs also suffer from saddle point in cases of headon approach towards an unsafe zone. We show that CBF-on-V avoids this pitfall entirely.
Aditya Vats, Tianyi Xia, Nadia Figueroa
The authors are with the University of Pennsylvania, Philadelphia, PA, USA.
Reactive task-space planners such as Bug2 operate with fixed Cartesian step sizes and are unaware of the manipulator's joint-angle limits. When the Jacobian is poorly conditioned, even small Cartesian steps can demand joint changes that exceed admissible bounds; clipping the joints to their limits causes tracking drift and can prevent goal reaching entirely. We address this by computing, at each planning step, the largest Cartesian hyperrectangle that is certifiably reachable under joint displacement bounds. Using a second-order polynomial approximation of the inverse kinematics and the S-procedure, we formulate a small semidefinite program whose solution yields the certified half-width~λ⋆. An equivalent bisection procedure exploiting the quadratic structure solves the certification in sub-millisecond time. Integrating this certificate with Bug2 yields a planner whose step size adapts to local kinematic conditioning. In a statistical evaluation over 94 adversarial scenarios spanning six joint-limit settings, the SOS-verified planner achieves zero joint-limit violations with a 100% goal-reaching rate, whereas a standard Bug2 planner violates joint limits in 6--11% of steps and fails to reach the goal in up to 18% of scenarios.
Hanjiang Hu, Changliu Liu, Yebin Wang
Robotics Institute, Carnegie Mellon University · Mitsubishi Electric Research Laboratories (MERL), Cambridge, MA 02139, USA