Literature
Display
← All papers

A New Semidefinite Relaxation for Linear and Piecewise-Affine Optimal Control with Time Scaling

Lujie Yang · Tobia Marcucci · Pablo A. Parrilo · Russ Tedrake

· ICRA 2025 ↗

Robotics6 min text · CondensedOriginal paper ↗
Contents

At a glance

The paper relaxes optimal control with jointly optimized trajectories and discretization time steps into a semidefinite program. A selective, time-scaled lifting captures the relevant polynomial products without constructing a full second-order moment matrix. Sparse variants reduce matrix size, and a graph of convex sets extends the formulation to piecewise-affine mode sequences. Experiments show useful lower bounds and initializations, but exactness is empirical rather than guaranteed; dynamically feasible trajectories are recovered with nonlinear refinement.

I Introduction

Variable time steps make even discretized linear dynamics bilinear in state, control, and time. Alternating optimization or direct collocation can find local solutions, while mixed-integer mode scheduling can become expensive when modes are assigned at every time step. The proposed time-flexible relaxation exploits the structure of these products and embeds mode-level subproblems in a graph. Its contribution is a compact relaxation of a discretized model, not a general proof of globally optimal continuous-time robot control.

Equation 1
xk+1=xk+h⁡(A​xk+B​uk),x_{k+1}=x_{k}+h(Ax_{k}+Bu_{k}), (1)
Refer to caption
Fig. 1: Minimum-time trajectories for a 7D double integrator moving among obstacles with robot arm geometry. The collision-free configuration space is approximately decomposed into convex polytopes. Our algorithm efficiently navigates these polytopes while respecting input limits. Videos of the trajectories can be found at (left) and (right).

II Preliminaries on Semidefinite Relaxation

A standard semidefinite relaxation [35] lifts quadratic products into a positive semidefinite matrix and drops the rank-one constraint. The relaxed optimum is a lower bound on the original minimization problem. A rank-one solution can recover an exact original solution; otherwise feasibility and optimality require additional analysis or recovery. This distinction remains central when the paper combines relaxation with nonlinear trajectory refinement.

Equations 2a–3f
minimizex⊤​A0​x\displaystyle\minimize\quad x^{\top}A_{0}x (2a)
subject tox⊤​Ai​x\displaystyle\text{subject to}\quad x^{\top}A_{i}x ≥0,∀i=1,…,l\displaystyle\geq 0,\quad\forall i=1,\ldots,l (2b)
B​x\displaystyle Bx ≥0\displaystyle\geq 0 (2c)
x1\displaystyle x_{1} =1,\displaystyle=1, (2d)
minimizetr⁡(A0​X)\displaystyle\minimize\quad\tr(A_{0}X) (3a)
subject totr⁡(Ai​X)\displaystyle\text{subject to}\quad\tr(A_{i}X) ≥0,∀i=1,…,l\displaystyle\geq 0,\quad\forall i=1,\ldots,l (3b)
B​X​e1\displaystyle BXe_{1} ≥0\displaystyle\geq 0 (3c)
B​X​B⊤\displaystyle BXB^{\top} ≥0\displaystyle\geq 0 (3d)
e1⊤​X​e1\displaystyle e_{1}^{\top}Xe_{1} =1\displaystyle=1 (3e)
X\displaystyle X ⪰0,\displaystyle\succeq 0, (3f)

III Problem Statement

The decision variables are a fixed-length sequence of states and controls plus a positive common time step. States and inputs lie in prescribed polytopes, with fixed start and goal states. The objective combines total duration with time-weighted quadratic state and input penalties. Euler-discretized dynamics and the chosen horizon define the optimization model; their discretization error is separate from the semidefinite relaxation gap.

Equations 4a–4d
minimize\displaystyle\minimize\quad η​K​h+h​∑k=0K−1(xk⊤​Q​xk+uk⊤​R​uk)\displaystyle\eta Kh+h\sum_{k=0}^{K-1}(x_{k}^{\top}Qx_{k}+u_{k}^{\top}Ru_{k}) (4a)
subject to xk+1=f(xk,uk,h),∀k=0,…,K−1\displaystyle x_{k+1}=f(x_{k},u_{k},h),\quad\forall k=0,\ldots,K-1 (4b)
xk∈𝒳,uk∈𝒰,∀k=0,…,K−1\displaystyle x_{k}\in\mathcal{X},\ u_{k}\in\mathcal{U},\quad\forall k=0,\ldots,K-1 (4c)
x0=xstart,xK=xgoal,\displaystyle x_{0}=x_{\text{start}},\;x_{K}=x_{\text{goal}}, (4d)

IV Convex Relaxation with Time Scaling

The method first handles linear time-invariant dynamics, then fixed piecewise-affine mode sequences, and finally mode selection. Its main design choice is which time–trajectory products to lift and which redundant original constraints to retain as useful convex restrictions.

IV-A Time-Flexible Relaxation

Collect all trajectory variables in a vector r and lift the vector containing 1, h, r, and hr into a matrix scaled by 1/h. Positivity of h makes this a valid positive semidefinite construction. It retains products needed for both bilinear dynamics and cubic time-weighted costs while omitting the full set of second-order monomials. Multiplying original linear constraints produces additional equalities and inequalities that are redundant before relaxation but tighten the lifted feasible set. The appendix gives the full program.

Equation 5
y=[1hrh​r],Y=1h​y​y⊤=[1h1r⊤hr⊤1hr⊤h​r⊤rhrr​r⊤hr​r⊤rh​rr​r⊤h​r​r⊤].y=\begin{bmatrix}1\\ h\\ r\\ hr\end{bmatrix},\quad Y=\frac{1}{h}yy^{\top}=\begin{bmatrix}\frac{1}{h}&1&\frac{r^{\top}}{h}&r^{\top}\\ 1&h&r^{\top}&hr^{\top}\\ \frac{r}{h}&r&\frac{rr^{\top}}{h}&rr^{\top}\\ r&hr&rr^{\top}&hrr^{\top}\end{bmatrix}. (5)

IV-B Sparse Time-Flexible Relaxation

A dense lifted matrix grows with the entire horizon. The sparse formulation instead uses local blocks for adjacent states and the current input, together with consistency constraints for shared states and their lifted products. On the minimum-time double-integrator example, a standard first-order relaxation is loose, while a full second-order matrix would have dimension 4,465 and exhausts memory. Dense TFR uses dimension 186; the reported sparse construction uses 29 matrices of dimension 12 and closely follows the optimal policy. These are example-specific size and tightness results.

Equations 6–7
rk=[xkxk+1uk],yk=[1hrkh​rk],Yk=1h​yk​yk⊤.r_{k}=\begin{bmatrix}x_{k}\\ x_{k+1}\\ u_{k}\end{bmatrix},\quad y_{k}=\begin{bmatrix}1\\ h\\ r_{k}\\ hr_{k}\end{bmatrix},\quad Y_{k}=\frac{1}{h}y_{k}y_{k}^{\top}. (6)
rk[nx:2nx]=rk+1[:nx],r_{k}[n_{x}:2n_{x}]=r_{k+1}[:n_{x}], (7)
Refer to caption
Fig. 2: Comparison of different SDRs for the minimum-time double integrator. (a) Ground truth state-space trajectories. (b) Trajectories from dense TFR. (c) Trajectories from sparse TFR. (d) Trajectories from standard SDR.

V Piecewise-Affine Systems with a Fixed Mode Sequence

For a fixed mode sequence, each segment has its own trajectory variables and time step. The formulation relaxes each segment separately, imposes boundary conditions at the endpoints, and couples successive segments through state continuity. This modular structure prepares mode-level graph optimization. The fixed-sequence problem itself has no mode-choice integer variables; the computational benefit concerns the subsequent mode-scheduling formulation.

Equations 8–9e
xk+1=xk+h⁡(Ai​xk+Bi​uk+ci)if​(xk,uk)∈𝒳i×𝒰i,x_{k+1}=x_{k}+h(A_{i}x_{k}+B_{i}u_{k}+c_{i})\\ \text{if}\ (x_{k},u_{k})\in\mathcal{X}_{i}\times\mathcal{U}_{i}, (8)
minimize\displaystyle\minimize\quad ∑n=1Nhn​(η​Kn+∑k=0Kn−1(xkn⊤​Q​xkn+ukn⊤​R​ukn))\displaystyle\sum_{n=1}^{N}h_{n}\left(\eta K_{n}+\sum_{k=0}^{K_{n}-1}({x_{k}^{n}}^{\top}Qx_{k}^{n}+{u_{k}^{n}}^{\top}Ru_{k}^{n})\right) (9a)
subject to ∀n=1,…,N,∀k=0,…,Kn−1:\displaystyle\forall n=1,\ldots,N,\ \forall k=0,\ldots,K_{n}-1:
xk+1n=xkn+hn​(Ain​xkn+Bin​ukn+cin)\displaystyle x_{k+1}^{n}=x_{k}^{n}+h_{n}(A_{i_{n}}x_{k}^{n}+B_{i_{n}}u_{k}^{n}+c_{i_{n}}) (9b)
xkn∈𝒳in,ukn∈𝒰in\displaystyle x_{k}^{n}\in\mathcal{X}_{i_{n}},\ u_{k}^{n}\in\mathcal{U}_{i_{n}} (9c)
x0n=xKn−1n−1\displaystyle x_{0}^{n}=x_{K_{n-1}}^{n-1} (9d)
x01=xstart,xKNN=xgoal.\displaystyle x_{0}^{1}=x_{\text{start}},\;x_{K_{N}}^{N}=x_{\text{goal}}. (9e)

V-A Continuity Constraints with Coupling Matrix

A coupling positive semidefinite matrix contains the shared boundary state together with the time variables from neighboring modes. Matching its entries to the segment matrices aligns both ordinary state moments and time-dependent products, making continuity stronger than simply matching the first-moment state. In the linearized inverted-pendulum example with elastic walls, these constraints tighten the lower bound across the tested initial conditions.

Equations 10
x¯\displaystyle\bar{x} =xKnn=x0n+1,Y¯=y¯​y¯⊤\displaystyle=x_{K_{n}}^{n}=x_{0}^{n+1},\ \bar{Y}=\bar{y}\bar{y}^{\top} (10)
y¯\displaystyle\bar{y} =(1,hn,hn+1,x¯,hn​x¯,hn+1​x¯,x¯hn,x¯hn+1).\displaystyle=\left(1,h_{n},h_{n+1},\bar{x},h_{n}\bar{x},h_{n+1}\bar{x},\frac{\bar{x}}{h_{n}},\frac{\bar{x}}{h_{n+1}}\right).
x¯​x¯⊤,hn​x¯,hn+1​x¯,x¯/hn,x¯/hn+1,\displaystyle\bar{x}\bar{x}^{\top},\ h_{n}\bar{x},\ h_{n+1}\bar{x},\ \bar{x}/h_{n},\ \bar{x}/h_{n+1},
hn​x¯​x¯⊤,hn+1​x¯​x¯⊤,x¯​x¯⊤/hn,x¯​x¯⊤/hn+1,\displaystyle h_{n}\bar{x}\bar{x}^{\top},\ h_{n+1}\bar{x}\bar{x}^{\top},\ \bar{x}\bar{x}^{\top}/h_{n},\ \bar{x}\bar{x}^{\top}/h_{n+1},
Refer to caption
Fig. 3: Effect of the coupling variable on balancing the inverted pendulum with the wall in minimum time for different initial conditions. The vertical dashed line indicates the angular limit beyond which the pendulum collides with the wall. The legend indicates the time duration for each trajectory.

VI Mode Optimization for Piecewise-Affine Systems

Mode sequences become paths through a directed graph, and each mode contributes a convex feasible set describing its relaxed dynamics. The combined formulation relaxes both continuous nonconvex dynamics and discrete path selection.

VI-A Shortest Paths in Graphs of Convex Sets

In a graph of convex sets [27], each vertex carries a continuous variable in a convex set; edge costs and compatibility constraints depend on endpoint variables. Finding the shortest discrete path remains NP-hard. The paper uses a convex relaxation of this graph problem, whose tightness is assessed empirically. A single convex solve therefore returns a relaxed solution, not an unconditional exact solution to arbitrary mode scheduling.

Equations 11a–11d
minimize\displaystyle\minimize\quad ∑e=(u,v)∈ℰple​(xu,xv)\displaystyle\sum_{e=(u,v)\in\mathcal{E}_{p}}l_{e}(x_{u},x_{v}) (11a)
subject to p∈𝒫\displaystyle p\in\mathcal{P} (11b)
xv∈𝒳v,\displaystyle x_{v}\in\mathcal{X}_{v}, ∀v∈𝒱\displaystyle\quad\forall v\in\mathcal{V} (11c)
(xu,xv)∈𝒳e,\displaystyle(x_{u},x_{v})\in\mathcal{X}_{e},\quad ∀e=(u,v)∈ℰp,\displaystyle\forall e=(u,v)\in\mathcal{E}_{p}, (11d)

VI-B Spectrahedra as Convex Sets 𝒳 v

The feasible region of each mode’s semidefinite program is a spectrahedron. These regions become graph-vertex sets, and their lifted matrices become the continuous graph variables. Edge constraints impose the mode-to-mode compatibility conditions.

VI-C Optimal Control of PWA Systems with Time Scaling

Solve the combined graph and time-flexible relaxation to obtain a mode sequence and approximate trajectory, then initialize a nonlinear solver on the original bilinear problem for that fixed sequence. This refinement restores dynamical consistency and supplies a feasible upper-bound cost. Comparing it with the relaxed lower bound quantifies the remaining optimality gap; local refinement alone does not prove a global optimum.

VII Additional Experimental Results

The experiments test a linearized contact pendulum and obstacle-aware planning for a seven-dimensional double integrator with robot-arm geometry. They study trajectory quality and relaxation tightness within these models.

VII-A Inverted Pendulum with Elastic Wall

The contact-pendulum comparison uses 20 steps per mode with optimized durations, versus a fixed-step mixed-integer formulation with h=0.005 and 160 steps. The proposed relaxed trajectories closely follow nonlinear-refined solutions in the illustrated cases; the mixed-integer relaxation often misses wall contacts. Figures also evaluate a combined time-and-control-effort objective. Because the discretizations and timing freedom differ, their objectives should be interpreted within the corresponding formulations. The reported gap compares a relaxed lower bound with a feasible refined or mixed-integer solution, rather than directly measuring continuous-time physical optimality.

Refer to caption
Fig. 4: Time-optimal trajectories for the inverted pendulum with wall. (a) GCS+TFR+NLP solutions. The vertical dashed line represents the angle at which the pendulum makes contact with the wall. (b) GCS+TFR solutions. (c) Convex relaxation of MICP formulation with fixed hh. The different segment colors indicate mode transitions.
Refer to caption
Fig. 5: Performance comparison for balancing the inverted pendulum with contact in minimum time. The pendulum starts from the initial conditions (0.09,θ˙0)(0.09,\dot{\theta}_{0}). (a) Final time. (b) Relaxation gap. The proposed GCS+TFR formulation is much tighter than the relaxation of the standard MICP.
Refer to caption
Fig. 6: Trajectories for balancing the inverted pendulum with the time and control-effort cost. (a) GCS+TFR+NLP solutions. (b) GCS+TFR solutions. (c) Convex relaxation of MICP.
Refer to caption
Fig. 7: Performance comparison for balancing the inverted pendulum with a time and a control effort penalty.

VII-B Robot Arm

The arm experiment uses a seven-dimensional double-integrator approximation with LBR iiwa collision geometry, not full manipulator dynamics. IRIS-NP [49] approximately covers collision-free configuration space with ten convex regions. The planner selects a region sequence and a trajectory within that decomposition. Across five tasks, the tested direct-collocation setup fails to find collision-free paths, even with TFR initialization; this is a result for that implementation and problem setup.

Table I reports relaxation gaps of 0.9%, 2.1%, 14.3%, 24.4%, and 5.8%. Thus several solutions are close to their lower bounds, but the arm results do not establish uniformly near-zero gaps or exact global optimality. Full robot dynamics and coverage beyond the chosen convex-region decomposition remain outside the demonstrated model.

Refer to caption
Fig. 8: Minimum-time trajectories for the robot arm. The trajectories generated by our method (orange) successfully navigate the constrained environment, while those computed using direct collocation (blue) constantly result in collisions.Videos of the trajectories can be found at (a), (b), (c).
Task 0 1 2 3 4
Relaxation gap δ\delta (%) 0.9 2.1 14.3 24.4 5.8
TABLE I: Relaxation gap of the TFR for moving the robot arm to different configurations in minimum time.

VIII Conclusion

The selective time-scaled lifting offers a useful compromise between weak first-order relaxations and expensive full higher-order relaxations. Its sparse and graph-based extensions support variable-duration hybrid planning, with nonlinear recovery and explicit gap estimates defining the practical guarantees. Empirical tightness should not be read as a theorem of exactness.

Appendix: complete relaxation. Equations 12–13 express dynamics, polytope constraints, and terminal conditions in lifted coordinates. They include the original constraints, products with trajectory and time variables, and scaled forms involving h and 1/h. Products of nonnegative linear inequalities add entrywise nonnegativity restrictions. Equation 14 matches shared-state moments across sparse blocks, including ordinary, time-weighted, and inverse-time-weighted products. All these original equations are retained below; they specify the tightening constraints essential to reproducing the relaxation.

Equations 12a–14
minimize\displaystyle\minimize\ η​K​h+tr⁡((Qx+Ru)⋅h​r​r⊤)\displaystyle\eta Kh+\tr((Q_{x}+R_{u})\cdot hrr^{\top}) (12a)
subject to F​r=G​h​r(dynamical constraints)\displaystyle Fr=Ghr\quad\;\;\text{(dynamical constraints)} (12b)
C​r+d≥0(input and state constraints)\displaystyle Cr+d\geq 0\quad\;\text{(input and state constraints)} (12c)
H​r=b(terminal conditions)\displaystyle Hr=b\quad\quad\quad\text{(terminal conditions)} (12d)
h≥0.\displaystyle h\geq 0. (12e)
minimize\displaystyle\minimize\ η​K​h+tr⁡((Qx+Ru)⋅h​r​r⊤)\displaystyle\eta Kh+\tr((Q_{x}+R_{u})\cdot hrr^{\top}) (13a)
subject to F​r=G​h​r,F​r/h=G​r\displaystyle Fr=Ghr,\ Fr/h=Gr (13b)
Frr⊤=Ghrr⊤,Frr⊤/h=Grr⊤\displaystyle Frr^{\top}=Ghrr^{\top},\ Frr\top/h=Grr^{\top} (13c)
H​r=b,H​h​r=b​h\displaystyle Hr=b,\ Hhr=bh (13d)
H​r/h=b/h,H​r​r⊤=b​r⊤\displaystyle Hr/h=b/h,\ Hrr^{\top}=br^{\top} (13e)
Hhrr⊤=bhr⊤,Hrr⊤/h=br⊤/h\displaystyle Hhrr^{\top}=bhr^{\top},\ Hrr\top/h=br^{\top}/h (13f)
C​r+d≥0,(C​r+d)​(C​r+d)⊤≥0\displaystyle Cr+d\geq 0,\ (Cr+d)(Cr+d)^{\top}\geq 0 (13g)
h⁡(C​r+d)≥0,h⁡(C​r+d)​(C​r+d)⊤≥0\displaystyle h(Cr+d)\geq 0,\ h(Cr+d)(Cr+d)^{\top}\geq 0 (13h)
(C​r+d)/h≥0,(C​r+d)​(C​r+d)⊤/h≥0\displaystyle(Cr+d)/h\geq 0,\ (Cr+d)(Cr+d)^{\top}/h\geq 0 (13i)
h≥0, 1/h≥0,\displaystyle h\geq 0,\ 1/h\geq 0, (13j)
hrk[nx:2nx]\displaystyle hr_{k}\left[n_{x}:2n_{x}\right] =hrk+1[:nx]\displaystyle=hr_{k+1}[:n_{x}] (14)
rkh[nx:2nx]\displaystyle\frac{r_{k}}{h}\left[n_{x}:2n_{x}\right] =rk+1h[:nx]\displaystyle=\frac{r_{k+1}}{h}[:n_{x}]
rkrk⊤[nx:2nx,nx:2nx]\displaystyle r_{k}r_{k}^{\top}\left[n_{x}:2n_{x},n_{x}:2n_{x}\right] =rk+1rk+1⊤[:nx,:nx]\displaystyle=r_{k+1}r_{k+1}^{\top}[:n_{x},:n_{x}]
hrkrk⊤[nx:2nx,nx:2nx]\displaystyle hr_{k}r_{k}^{\top}\left[n_{x}:2n_{x},n_{x}:2n_{x}\right] =hrk+1rk+1⊤[:nx,:nx]\displaystyle=hr_{k+1}r_{k+1}^{\top}[:n_{x},:n_{x}]
rk​rk⊤h[nx:2nx,nx:2nx]\displaystyle\frac{r_{k}r_{k}^{\top}}{h}\left[n_{x}:2n_{x},n_{x}:2n_{x}\right] =rk+1​rk+1⊤h[:nx,:nx].\displaystyle=\frac{r_{k+1}r_{k+1}^{\top}}{h}[:n_{x},:n_{x}].
Concept demonstration

Open image ↗