Literature
Display
← All papers

Semidefinite Relaxations for Collision-Free Motion Planning

Bernhard Paus Graesdal · Alexandre Amice · Pablo A. Parrilo · Russ Tedrake

Preprint posted · arXiv (preprint) ↗

Robotics10 min text · CondensedOriginal paper ↗
Contents

At a glance

The paper studies polynomial point-robot trajectories among spherical obstacles, with derivative costs and boundary/continuity constraints. Exact sum-of-squares collision certificates lead to a semidefinite relaxation whose slack has a geometric meaning: it permits motion in extra dimensions. This characterizes tightness and supports a smaller, symmetry-reduced formulation. Experiments show much faster, more consistent solves than the tested nonlinear baselines and useful RRT steering. A loose relaxation can project through obstacles; global optimality of the original trajectory requires the stated tightness and recovery conditions.

I Introduction

Collision avoidance remains nonconvex even for a point robot, while smoothness and boundary constraints complicate geometric planning. The approach combines exact whole-curve collision certificates with a first-order convex relaxation, then uses sampling to supply global exploration. Unlike time-sampled collision constraints, the certificate covers every point on a fixed polynomial segment. The theoretical contribution explains when lifting preserves the original optimum and when extra-dimensional shortcuts make it loose, building on the SOS/moment framework.

Refer to caption
Fig. 1: A collision-free, C4C^{4}-continuous, minimum-snap quadrotor trajectory through a cluttered field of spherical obstacles, planned by an RRT that uses our semidefinite relaxation as a convex steering function. Our relaxation considers both obstacle-avoidance and other trajectory constraints, while being fast and reliable enough to make optimization a viable inner loop. The color coding of the trajectory indicates velocity (red is slow, green is fast).

II Related Works

Prior work applied moment/Shor relaxations to dynamics, collision avoidance, time scaling and contact, often observing tightness empirically. This paper deliberately restricts geometry and cost to analyze tightness rather than assume it. Its symmetry reduction addresses ambient-coordinate redundancy and can coexist with sparsity across trajectory segments. The analyzed setting does not encompass arbitrary articulated robots or general nonlinear dynamics.

III Motion Planning

The initial problem optimizes a continuously differentiable curve, with obstacle inequalities required at every parameter value. Restricting to finitely many polynomial coefficients gives a finite-dimensional decision variable but leaves infinitely many collision constraints. The SOS construction removes those continuum constraints exactly within the selected polynomial class.

III-A Problem Statement

Minimize the integral of the squared kth derivative over [0,1], subject to Cη continuity, prescribed endpoint derivatives through order ℓ, and distance at least each obstacle radius. Examples are velocity/energy, acceleration and snap costs. Exactness later refers to this polynomial, fixed-parameter-domain formulation, not automatically the unrestricted infinite-dimensional or free-time optimum.

Equations 1a–1f
min\displaystyle\min\quad ∫01‖q(k)​(s)‖22​𝑑s\displaystyle\int_{0}^{1}\|q^{(k)}(s)\|_{2}^{2}\,ds (1a)
s.t.\displaystyle\subto\quad ‖q⁡(s)−cj‖22−rj2≥0,∀s∈[0,1],\displaystyle\|q(s)-c_{j}\|_{2}^{2}-r_{j}^{2}\geq 0,\quad\forall s\in[0,1], (1b)
j=1,…,m,\displaystyle\quad\quad\quad\quad\quad\quad\quad\quad\quad\;\ j=1,\ldots,m, (1c)
q(i)​(0)=q0(i),\displaystyle q^{(i)}(0)=q_{0}^{(i)}, (1d)
q(i)(1)=q1(i),i=0,…,ℓ\displaystyle q^{(i)}(1)=q_{1}^{(i)},\quad i=0,\ldots,\ell (1e)
q∈𝒞η,\displaystyle q\in\mathcal{C}^{\eta}, (1f)

III-B Polynomial Curves

A segment is γ(s)=Γbᵈ(s), with coefficient matrix Γ of size n×(d+1). Costs and obstacle conditions separate by segment; linear derivative-matching constraints couple neighboring segments. Derivations are mostly basis-independent, with Bernstein/Bézier coordinates used in the implementation.

Equation 2
γ⁡(s)=Γ​bd​(s),\displaystyle\gamma(s)=\Gamma b_{d}(s), (2)

III-C Curve Costs

Squared derivative cost is tr(G⁽ᵏ⁾ΓᵀΓ), where the derivative Gram matrix is analytic for a chosen basis. Its rank is d+1−k and its nullspace consists of polynomials below degree k. These zero-cost directions explain why endpoint conditions matter for uniqueness.

Equations 3–5
∫01‖γ(k)​(s)‖22​𝑑s=tr⁡(Gd(k)​ΓT​Γ),\int_{0}^{1}\|\gamma^{(k)}(s)\|_{2}^{2}\,ds=\mathrm{tr}(G^{(k)}_{d}\,\Gamma^{T}\Gamma), (3)
Gd(k)=Dk​Gd−k​DkT∈𝕊d+1\displaystyle G^{(k)}_{d}=D_{k}G_{d-k}D_{k}^{T}\in\mathbb{S}^{d+1} (4)
Gd=∫01bd​(s)​bd​(s)T​𝑑sG_{d}=\int_{0}^{1}b_{d}(s)\,b_{d}(s)^{T}\,ds (5)

III-D Semi-Infinite QCQP Reformulation

For a unit sphere, squared distance minus one is a univariate polynomial of degree at most 2d. A coefficient vector u represents the constant polynomial one, giving the shifted matrix Γ−cuᵀ. The resulting problem has quadratic cost/obstacle terms and linear boundary equation ΓA=B, but still requires polynomial nonnegativity for every s.

Equations 6–9
p⁡(s):=‖γ⁡(s)−c‖22−1≥0∀s∈[0,1].\displaystyle p(s):=\|\gamma(s)-c\|_{2}^{2}-1\geq 0\quad\forall s\in[0,1]. (6)
p⁡(s)\displaystyle p(s) =bd​(s)T​((Γ−c​uT)T​(Γ−c​uT)−u​uT)​bd​(s)\displaystyle=b_{d}(s)^{T}\big((\Gamma-cu^{T})^{T}(\Gamma-cu^{T})-uu^{T}\big)b_{d}(s) (7)
≥0∀s∈[0,1],\displaystyle\geq 0\quad\forall s\in[0,1],
min\displaystyle\min\quad tr⁡(G(k)​ΓT​Γ)\displaystyle\mathrm{tr}(G^{(k)}\Gamma^{T}\Gamma) (8)
s.t.\displaystyle\subto\quad bd​(s)T​((Γ−c​uT)T​(Γ−c​uT)−u​uT)​bd​(s)≥0\displaystyle b_{d}(s)^{T}\big((\Gamma-cu^{T})^{T}(\Gamma-cu^{T})-uu^{T}\big)b_{d}(s)\geq 0
∀s∈[0,1],\displaystyle\quad\quad\quad\forall s\in[0,1],
Γ​A=B,\displaystyle\Gamma A=B,
A\displaystyle A =[bd​(0)bd​(1)…bd(ℓ)​(0)bd(ℓ)​(1)],\displaystyle=\begin{bmatrix}b_{d}(0)&b_{d}(1)&\ldots&b_{d}^{(\ell)}(0)&b_{d}^{(\ell)}(1)\end{bmatrix},
B\displaystyle B =[q0q1…q0(ℓ)q1(ℓ)].\displaystyle=\begin{bmatrix}q_{0}&q_{1}&\ldots&q_{0}^{(\ell)}&q_{1}^{(\ell)}\end{bmatrix}. (9)

IV Certifying Collision-Avoidance

A univariate polynomial’s nonnegativity on an interval has an exact SOS representation. Certifying a fixed path is convex; searching its coefficients remains nonconvex because the certificate depends quadratically on Γ.

IV-A Basis-Independent Derivation

Markov–Lukács writes an interval-nonnegative degree-2d polynomial as σ₀+(s−a)(b−s)σ₁, with SOS degrees at most 2d and 2d−2. Coefficient matching yields PSD Gram matrices of sizes d+1 and d, independent of ambient dimension n. The same fixed-curve certificate extends to ellipsoids through (Γ−cuᵀ)ᵀE(Γ−cuᵀ). The later geometric tightness analysis concerns spherical obstacles, not a proved general ellipsoid-planning theorem.

Equations 10–12
p⁡(s)=σ0​(s)+(s−a)​(b−s)​σ1​(s),\displaystyle p(s)=\sigma_{0}(s)+(s-a)(b-s)\sigma_{1}(s), (10)
ℒ⁡((Γ−c​uT)T​(Γ−c​uT),Q0,Q1)=0,\displaystyle\mathcal{L}((\Gamma-cu^{T})^{T}(\Gamma-cu^{T}),Q_{0},Q_{1})=0, (11)
p⁡(s)−σ0​(s)−s⁡(1−s)​σ1​(s)=0,\displaystyle p(s)-\sigma_{0}(s)-s(1-s)\sigma_{1}(s)=0,
ℒ⁡((Γ−c​uT)T​E​(Γ−c​uT),Q0,Q1)=0,\displaystyle\mathcal{L}((\Gamma-cu^{T})^{T}E(\Gamma-cu^{T}),Q_{0},Q_{1})=0, (12)

IV-B Collision-Avoidance with Bézier Curves

Dropping binomial factors gives a scaled Bernstein basis whose products simply add indices. The map S sums matrix antidiagonals; T shifts the degree-(d−1) map to account for s(1−s). Their coefficient identity is an exact collision certificate. The implementation uses standard Bernstein coordinates for conditioning, with weighted maps supplied in the appendix.

Equations 13–16
Bid(s):=si(1−s)d−i,i=0,…,d,\displaystyle B_{i}^{d}(s):=s^{i}(1-s)^{d-i},\quad i=0,\ldots,d, (13)
∑i=0d(di)​si​(1−s)d−i=1,\displaystyle\sum_{i=0}^{d}\binom{d}{i}s^{i}(1-s)^{d-i}=1,
Bid​(s)⋅Bjd​(s)=Bi+j2​d​(s),\displaystyle B_{i}^{d}(s)\cdot B_{j}^{d}(s)=B_{i+j}^{2d}(s),
bd​(s)T​M​bd​(s)\displaystyle b_{d}(s)^{T}Mb_{d}(s) =∑i,jMi​j​Bid​(s)​Bjd​(s)=∑i,jMi​j​Bi+j2​d​(s)\displaystyle=\sum_{i,j}M_{ij}B_{i}^{d}(s)B_{j}^{d}(s)=\sum_{i,j}M_{ij}B_{i+j}^{2d}(s)
=𝒮d​(M)T​b2​d​(s),\displaystyle=\mathcal{S}_{d}(M)^{T}b_{2d}(s),
[𝒮d​(M)]k=∑i+j=kMi​j.\displaystyle[\mathcal{S}_{d}(M)]_{k}=\sum_{\begin{subarray}{c}i+j=k\end{subarray}}M_{ij}. (14)
s⁡(1−s)⋅Bid−1​(s)⋅Bjd−1​(s)=Bi+j+12​d​(s),\displaystyle s(1-s)\cdot B_{i}^{d-1}(s)\cdot B_{j}^{d-1}(s)=B_{i+j+1}^{2d}(s),
s⁡(1−s)​bd−1​(s)T​M​bd−1​(s)=𝒯d​(M)T​b2​d​(s),\displaystyle s(1-s)b_{d-1}(s)^{T}Mb_{d-1}(s)=\mathcal{T}_{d}(M)^{T}b_{2d}(s),
[𝒯d​(M)]k={[𝒮d−1​(M)]k−1if ​1≤k≤2​d−1,0otherwise.\displaystyle[\mathcal{T}_{d}(M)]_{k}=\begin{cases}[\mathcal{S}_{d-1}(M)]_{k-1}&\text{if }1\leq k\leq 2d-1,\\ 0&\text{otherwise}.\end{cases} (15)
𝒮d​((Γ−c​uT)T​(Γ−c​uT)−u​uT−Q0)−𝒯d​(Q1)=0,\displaystyle\mathcal{S}_{d}\big((\Gamma-cu^{T})^{T}(\Gamma-cu^{T})-uu^{T}-Q_{0}\big)-\mathcal{T}_{d}(Q_{1})=0, (16)

IV-C The Special Case of Line Segments

For a line segment, eliminating one SOS matrix leaves a 2×2 PSD condition and a nonnegative scalar α, expressible as a rotated second-order cone. This reduces certificate cost; joint optimization of segment endpoints still contains their nonconvex quadratic geometry before relaxation.

Equations 17–18
γ⁡(s)=(1−s)​γ0+s​γ1,s∈[0,1],\displaystyle\gamma(s)=(1-s)\gamma_{0}+s\gamma_{1},\quad s\in[0,1], (17)
[γ0Tγ1T]​E​[γ0γ1]⪰[11+α1+α1],\displaystyle\begin{bmatrix}\gamma_{0}^{T}\\ \gamma_{1}^{T}\end{bmatrix}E\begin{bmatrix}\gamma_{0}&\gamma_{1}\end{bmatrix}\succeq\begin{bmatrix}1&1+\alpha\\ 1+\alpha&1\end{bmatrix}, (18)

V Semidefinite Relaxation

The exact finite-dimensional problem and its convex relaxation differ at the coefficient Gram-matrix equality.

V-A Nonconvex Problem

Replacing continuum collision constraints by SOS coefficient matching produces a finite problem over Γ,Q₀,Q₁. The SOS matrices are PSD, but coefficient matching is quadratic in Γ, so exact reformulation alone has not convexified planning.

Equation P
min\displaystyle\min\quad tr⁡(G(k)​ΓT​Γ)\displaystyle\mathrm{tr}(G^{(k)}\,\Gamma^{T}\Gamma) (P)
s.t.\displaystyle\subto\quad ℒ⁡((Γ−c​uT)T​(Γ−c​uT),Q0,Q1)=0,\displaystyle\mathcal{L}((\Gamma-cu^{T})^{T}(\Gamma-cu^{T}),Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
Γ​A=B,\displaystyle\Gamma A=B,

V-B Forming the Relaxation

Introduce X=ΓᵀΓ to make cost and collision matching affine, then relax equality to X⪰ΓᵀΓ through a Schur-complement block LMI. The implied relation XA=ΓᵀB tightens the relaxation and is essential for the lifted path’s endpoint conditions. The collision-certificate cones have sizes d+1 and d; the displayed joint Schur block is n+d+1, so the entire numerical formulation is not literally independent of n.

Equations 19–SDP
(Γ−c​uT)T​(Γ−c​uT)\displaystyle(\Gamma-cu^{T})^{T}(\Gamma-cu^{T}) =ΓT​Γ−(ΓT​c)​uT\displaystyle=\Gamma^{T}\Gamma-(\Gamma^{T}c)u^{T} (19)
−u​(ΓT​c)T+(cT​c)​u​uT.\displaystyle\quad-u(\Gamma^{T}c)^{T}+(c^{T}c)uu^{T}.
X⪰ΓT​Γ⇔[InΓΓTX]⪰0,\displaystyle X\succeq\Gamma^{T}\Gamma\iff\begin{bmatrix}I_{n}&\Gamma\\ \Gamma^{T}&X\end{bmatrix}\succeq 0, (20)
min\displaystyle\min\quad tr⁡(G(k)​X)\displaystyle\mathrm{tr}(G^{(k)}X) (SDP)
s.t.\displaystyle\subto\quad ℒ⁡(X−(ΓT​c)​uT−u​(ΓT​c)T\displaystyle\mathcal{L}(X-(\Gamma^{T}c)u^{T}-u(\Gamma^{T}c)^{T}
+(cT​c)​u​uT,Q0,Q1)=0,\displaystyle\quad\quad+(c^{T}c)uu^{T},Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
Γ​A=B\displaystyle\Gamma A=B
X​A=ΓT​B\displaystyle XA=\Gamma^{T}B
[InΓΓTX]⪰0.\displaystyle\begin{bmatrix}I_{n}&\Gamma\\ \Gamma^{T}&X\end{bmatrix}\succeq 0.

V-C Strict Feasibility and Facial Reduction

Boundary and tightening equations force the feasible set into a proper PSD-cone face, preventing strict feasibility in the unreduced form. In Bernstein coordinates, endpoint derivatives fix adjacent control points, allowing explicit elimination. With independent endpoint conditions, d+1−2(ℓ+1) free control points remain for a single segment, reducing the Schur block and removing known degeneracy.

Equation
⟨[B−A]​[BT−AT],[InΓΓTX]⟩=0,\displaystyle\left\langle\begin{bmatrix}B\\ -A\end{bmatrix}\begin{bmatrix}B^{T}&-A^{T}\end{bmatrix},\,\begin{bmatrix}I_{n}&\Gamma\\ \Gamma^{T}&X\end{bmatrix}\right\rangle=0,

V-D Extension to Multiple Segments

Multiple segments introduce within-segment and adjacent cross-Gram matrices. Cη matching supplies linear coefficient constraints and implied Gram constraints; these also permit facial reduction. The interior pairwise Schur block shrinks by η+1 columns, with further endpoint reductions. This preserves coupled smoothness rather than optimizing disconnected curves.

Equations 21
[XiXi,i+1Xi,i+1TXi+1]=[ΓiΓi+1]T​[ΓiΓi+1]\displaystyle\begin{bmatrix}X_{i}&X_{i,i+1}\\ X_{i,i+1}^{T}&X_{i+1}\end{bmatrix}=\begin{bmatrix}\Gamma_{i}&\Gamma_{i+1}\end{bmatrix}^{T}\begin{bmatrix}\Gamma_{i}&\Gamma_{i+1}\end{bmatrix}
[InΓiΓi+1ΓiTXiXi,i+1Γi+1TXi,i+1TXi+1]⪰0\displaystyle\begin{bmatrix}I_{n}&\Gamma_{i}&\Gamma_{i+1}\\ \Gamma_{i}^{T}&X_{i}&X_{i,i+1}\\ \Gamma_{i+1}^{T}&X_{i,i+1}^{T}&X_{i+1}\end{bmatrix}\succeq 0 (21)

VI Theoretical Results

The main results interpret positive slack as real motion in extra dimensions, characterize equality of optimum costs, and distinguish cost tightness from recovery of a particular path.

VI-A Geometric Interpretation

Factor the slack as VᵀV=X−ΓᵀΓ. The lifted trajectory (Γbᵈ,Vbᵈ) avoids spheres centered at (c,0), with V and its prescribed endpoint derivatives zero. Increasing the number ρ of extra dimensions cannot increase optimal cost; the SDP lower-bounds every lifted problem and attains one of them at ρ=rank(X−ΓᵀΓ). Thus it solves the cheapest member of that family. Independent boundary conditions bound ρ by the number of free control points. Only ρ=0 or 1 appeared in the reported experiments; a universal one-dimension bound is not proved.

Equation P ρ
min\displaystyle\min\quad tr⁡(G(k)​(ΓT​Γ+VT​V))\displaystyle\mathrm{tr}(G^{(k)}(\Gamma^{T}\Gamma+V^{T}V)) (Pρ)
s.t.\displaystyle\subto\quad ℒ⁡((Γ−c​uT)T​(Γ−c​uT)+VT​V,Q0,Q1)=0,\displaystyle\mathcal{L}((\Gamma-cu^{T})^{T}(\Gamma-cu^{T})+V^{T}V,Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
Γ​A=B,V​A=0.\displaystyle\Gamma A=B,\quad VA=0.
Refer to caption
Refer to caption
(a) Tight example.
Refer to caption
Refer to caption
(b) Not tight example.
Fig. 2: Geometric interpretation of the relaxation for two examples in ℝ2\mathbb{R}^{2}. Each example shows the trajectory defined by the polynomial coefficients Γ\Gamma from the solution to the relaxation in the original ambient space ℝ2\mathbb{R}^{2} (left), and the lifted trajectory defined by (Γ,V)(\Gamma,V) recovered from any factorization VT​V=X−ΓT​ΓV^{T}V=X-\Gamma^{T}\Gamma, following Theorem 3 (right). (a) Tight: X=ΓT​ΓX=\Gamma^{T}\Gamma, so V=0V=0 and the lifted trajectory coincides with the optimal ℝ2\mathbb{R}^{2} trajectory. (b) Not tight: ρ=rank⁡(X−ΓT​Γ)=1\rho=\operatorname{rank}(X-\Gamma^{T}\Gamma)=1, so V∈ℝ1×(d+1)V\in\mathbb{R}^{1\times(d+1)} is nonzero and the lifted trajectory uses the extra dimension to find a lower-cost trajectory that, when projected back to ℝ2\mathbb{R}^{2}, passes through the obstacles. The two trajectories shown are the two factorizations ±V\pm V of X−ΓT​ΓX-\Gamma^{T}\Gamma.

VI-B When is the Relaxation Tight?

Theorem 4 states that the SDP has the original optimal cost exactly when no number of extra dimensions permits a cheaper feasible path. This gives a geometric characterization, not a guarantee that an arbitrary environment is tight. Figure 2 contrasts a genuine planar optimum with a lifted shortcut whose planar projection collides.

VI-C Recovering a Minimizer

Theorem 5 adds a stronger recovery condition: every optimum of every lifted problem must have the same Γ and V=0. Then every SDP optimum recovers that path and X=ΓᵀΓ. Cost tightness alone need not identify a unique trajectory. Endpoint conditions eliminate derivative-cost null directions when k≤2(ℓ+1); obstacle symmetry can still create distinct equal-cost routes. Small geometric perturbations often remove that symmetry in practice.

VII Empirical Analysis

Random comparisons use 100 instances in R³ with fifteen obstacles and R⁵ with thirty, radii 0.3–1.0 in [−2,2]ⁿ. Nonlinear baselines use either twenty collision samples per segment or the exact SOS condition with PSD matrices factorized as LLᵀ. SNOPT/IPOPT start from a straight line, use feasibility tolerance 10⁻⁴ and a thirty-second timeout. All methods receive analogous equality-variable reduction. Table I varies degrees 1/3/5/7, derivative costs, continuity and segment counts; results depend on this setup.

Refer to caption
Refer to caption
Refer to caption
Fig. 3: Numerical evaluation of where the relaxation is tight and where it is loose for some representative obstacle configurations. The green region shows the set of reachable positions from the dark dot for which the relaxation is tight, and the red region shows where it is loose. The green lines show the trajectories obtained from solving the relaxation. Results are shown for minimum-energy cost with degree 33 polynomial trajectories.
Original figure
(a)
Median speedup over baseline (ℝ3\mathbb{R}^{3}, ℝ5\mathbb{R}^{5})
Baseline d=1d{=}1 d=3d{=}3 d=5d{=}5 d=7d{=}7
SNOPT (disc.) 15, 31 10, 15 14, 26 19, 28
SNOPT (exact) 39, 159 440, N/A N/A N/A
IPOPT (disc.) 84, 156 33, 61 57, 126 34, 70
IPOPT (exact) 14, 32 23, 30 71, 117 212, 397
(b)
Original figure
(c)
Fig. 4: Empirical comparison of the relaxation against SNOPT and IPOPT across random problem instances (see section VII-A). (a) Solve times (log scale); grey bars indicate all instances timed out. (b) Median speedup over each baseline, reported as (ℝ3,ℝ5)(\mathbb{R}^{3},\mathbb{R}^{5}). (c) Outcome breakdown against each baseline (left) and the distribution of the relaxation tightness (measured by ρ=rank⁡(X−ΓT​Γ)\rho=\operatorname{rank}(X-\Gamma^{T}\Gamma), see section VI) (right). The relaxation is 1–2 orders of magnitude faster than the local solvers with 1–3 orders of magnitude smaller variance.
Degree Cost Continuity BC Segments
1 velocity (k=1k=1) C0C^{0} 0 12
3 acceleration (k=2k=2) C2C^{2} 2 6
5 jerk (k=3k=3) C3C^{3} 3 4
7 snap (k=4k=4) C4C^{4} 4 3
TABLE I: Bézier curve configurations. Cost minimizes ∫‖γ(k)​(t)‖2​𝑑t\int\|\gamma^{(k)}(t)\|^{2}\,dt, polynomial degree is 2​k−12k-1, continuity is CkC^{k} (C0C^{0} for k=1k=1 to avoid overconstraining the problem), and BC order is the highest derivative fixed to zero at the endpoints.

VII-A Computational Cost

SDP median solves are 17–33 ms in R³ and 24–63 ms in R⁵; interquartile ranges are 1–4 and 1–12 ms. The sampled nonlinear baselines are generally tens of times slower, while exact nonlinear transcriptions can time out or become hundreds of times slower. Figure 4 retains the detailed ratios, including 212×/397× versus exact IPOPT at degree seven. Timed-out instances are excluded from the quoted nonlinear spread statistics, and an interquartile range should not be read as a formal variance estimate.

VII-B Empirical Tightness

Against the strongest baseline, 91% of the selected comparison instances have matching successful costs; the denominator includes only cases where the relaxation is tight or the baseline succeeds. It is not an unconditional 91% success rate. Figure 4 reports 88% rank-zero and 12% rank-one slack in R³, and 100% rank-zero in R⁵. The relationship between tightness and successful local optimization is empirical; loose cases still need global exploration or refinement.

VIII Kinodynamic Motion Planning

The relaxation supplies steering inside an RRT for degree-nine, C⁴-continuous minimum-snap trajectories. Convex steering can optimize smoothness and compatible convex trajectory constraints, while the sampler explores alternatives. A relaxation solve by itself is a collision-free original-space solution only when recovery/feasibility checks succeed; the paper’s earlier loose examples preclude an unconditional claim for every solve.

Refer to caption
Fig. 5: Minimum-snap trajectories (C4C^{4} continuous, degree 99 polynomials) found on 10 randomly generated ℝ2\mathbb{R}^{2} environments with 40 obstacles, by our RRT implementation that uses the SDP relaxation as the steering procedure in the Extend step and post-processes with a small convex program. Each subplot title reports the total planning time. Trajectories are colored by speed, from green (fast) to red (slow).

VIII-A RRT Implementation

SDP-RRT and NLP-RRT sample positions with endpoint derivatives left free; Geometric RRT finds waypoints before nonlinear smoothing. The nonlinear steering baseline uses smoothed signed-distance constraints, inflated obstacles, reduced collision sampling, adjusted tolerances and solver switching. SDP-RRT jointly reoptimizes the last two segments on each extension to control derivative growth, fixing derivatives when reaching the goal.

VIII-B Trajectory Post-Processing

SDP refinement adds sampled separating cuts between trajectory points and obstacles, resolves, and repeats until collision-free, then resamples to reduce cost. Once dense cuts make SOS constraints largely inactive, those constraints are dropped and each iteration becomes a small convex QP. This is explicitly heuristic post-processing; the exact whole-curve guarantee of the original SOS formulation should not be transferred to arbitrary sampled cuts without the stated collision verification.

VIII-C Numerical Results

On 100 planar environments with forty radius-one obstacles in a 15×15 square, SDP-RRT succeeds in 100 runs, NLP-RRT in 96 and geometric-plus-smoothing in 83. Median full times are 1.9, 22.9 and 2.7 seconds; median trajectory costs are 620, 455,000 and 991, respectively. Costs/times use each method’s successful runs, so their subsets differ. SDP steering totals 1.8 seconds versus 21.2 for NLP, excluding program-construction overhead. This benchmark has no extra dynamic or input bounds: support for such convex constraints is a capability of the formulation, not quantitatively demonstrated here.

Original figure
Fig. 6: Aggregate statistics showing median and interquartile ranges over 100 random environments for Geometric RRT, NLP-RRT, and SDP-RRT (ours), with costs and solve times reported over successful runs (nn per bar). SDP-RRT substantially outperforms NLP-RRT, and performs similar to Geometric RRT (with slightly higher success rate), which importantly, unlike our method, cannot incorporate additional constraints such as dynamics or input limits.

IX Conclusion

The relaxation is useful as a fast local component, yet can be loose where global routing matters. Open directions include higher-order moment relaxations, tighter slack-rank bounds, richer polynomial geometry, additional convex dynamics/input constraints and a structure-specialized solver. The evidence does not claim globally optimal arbitrary-robot motion planning or physical quadrotor deployment.

-A Connection To the Shor Relaxation

Vectorized Shor lifting uses Y⪰yyᵀ for y=vec(Γ), a matrix of size n(d+1). Decompose Y=yyᵀ+S: cost and collision equations depend on slack S only through blockwise partial traces. Orthogonal conjugation of each n×n block preserves feasibility and objective. Averaging over this symmetry restricts slack to S=(1/n)Λ⊗Iₙ, reducing its free PSD variable to size d+1. Substituting X=ΓᵀΓ+Λ recovers the proposed relaxation with tightening constraints. This is a symmetry of the slack construction, not an assumption that obstacle locations are rotationally symmetric.

Equations 22–24
MT​N=trn​(vec⁡(M)​vec​(N)T).\displaystyle M^{T}N=\mathrm{tr}_{n}(\mathrm{vec}(M)\mathrm{vec}(N)^{T}). (22)
min\displaystyle\min\quad tr⁡(G(k)​trn​(y​yT))\displaystyle\mathrm{tr}(G^{(k)}\,\mathrm{tr}_{n}(yy^{T}))
s.t.\displaystyle\subto\quad ℒ⁡(trn​(y​yT−(u⊗c)​yT−y​(u⊗c)T\displaystyle\mathcal{L}(\mathrm{tr}_{n}(yy^{T}-(u\otimes c)\,y^{T}-y\,(u\otimes c)^{T}
+(uuT)⊗(ccT)),Q0,Q1)=0,\displaystyle\quad\quad+(uu^{T})\otimes(cc^{T})),Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
(AT⊗In)​y=vec⁡(B),\displaystyle(A^{T}\otimes I_{n})\,y=\mathrm{vec}(B),
min\displaystyle\min\quad tr⁡(G(k)​trn​(Y))\displaystyle\mathrm{tr}(G^{(k)}\,\mathrm{tr}_{n}(Y)) (Shor)
s.t.\displaystyle\subto\quad ℒ⁡(trn​(Y−(u⊗c)​yT−y​(u⊗c)T\displaystyle\mathcal{L}(\mathrm{tr}_{n}(Y-(u\otimes c)y^{T}-y(u\otimes c)^{T}
+(uuT)⊗(ccT)),Q0,Q1)=0,\displaystyle\quad\quad+(uu^{T})\otimes(cc^{T})),Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
(AT⊗In)​y=vec⁡(B),\displaystyle(A^{T}\otimes I_{n})y=\mathrm{vec}(B),
(AT⊗In)​Y=vec⁡(B)​yT,\displaystyle(A^{T}\otimes I_{n})Y=\mathrm{vec}(B)\,y^{T},
[1yTyY]⪰0.\displaystyle\begin{bmatrix}1&y^{T}\\ y&Y\end{bmatrix}\succeq 0.
min\displaystyle\min\quad tr⁡(G(k)​trn​(y​yT+S))\displaystyle\mathrm{tr}(G^{(k)}\,\mathrm{tr}_{n}(yy^{T}+S)) (Shor′)
s.t.\displaystyle\subto\quad ℒ⁡(trn​(y​yT+S−(u⊗c)​yT\displaystyle\mathcal{L}(\mathrm{tr}_{n}(yy^{T}+S-(u\otimes c)y^{T}
−y​(u⊗c)T+(u​uT)⊗(c​cT)),\displaystyle\quad\quad-y(u\otimes c)^{T}+(uu^{T})\otimes(cc^{T})),
Q0,Q1)=0,\displaystyle\quad\quad Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
(AT⊗In)​y=vec⁡(B),\displaystyle(A^{T}\otimes I_{n})y=\mathrm{vec}(B),
(AT⊗In)​S=0,\displaystyle(A^{T}\otimes I_{n})S=0,
S⪰0.\displaystyle S\succeq 0.
TQ​(S)=(Id+1⊗QT)​S​(Id+1⊗Q)\displaystyle T_{Q}(S)=(I_{d+1}\otimes Q^{T})\,S\,(I_{d+1}\otimes Q) (23)
S¯=1n​Λ⊗In,Λ∈𝕊d+1,\displaystyle\bar{S}=\frac{1}{n}\Lambda\otimes I_{n},\quad\Lambda\in\mathbb{S}^{d+1}, (24)

-B Proofs for the Theoretical Results

Theorem 1 pads a lifted path with a zero coordinate. Theorem 2 projects the lifted SDP onto (Γ,X), or extends it with V=0, showing the same convex lower bound. Theorem 3 factors positive slack and uses XA=ΓᵀB together with ΓA=B to prove VA=0; the constructed path attains the bound. Lemma 4 bounds rank through the boundary matrix’s nullspace. Theorem 4 follows from both cost inequalities. Theorem 5 rules out nonzero slack because it would produce a forbidden lifted optimum under its uniqueness hypothesis. These are conditional exact results for the stated problem, not a proof that sampled experiments are always tight.

Equations SDP ρ
min\displaystyle\min\quad tr⁡(G(k)​X)\displaystyle\mathrm{tr}(G^{(k)}\,X) (SDPρ)
s.t.\displaystyle\subto\quad ℒ⁡(X−(ΓT​c)​uT−u​(ΓT​c)T\displaystyle\mathcal{L}(X-(\Gamma^{T}c)u^{T}-u(\Gamma^{T}c)^{T}
+(cT​c)​u​uT,Q0,Q1)=0,\displaystyle\qquad+(c^{T}c)uu^{T},Q_{0},Q_{1})=0,
Q0⪰0,Q1⪰0,\displaystyle Q_{0}\succeq 0,\;Q_{1}\succeq 0,
Γ​A=B,V​A=0\displaystyle\Gamma A=B,\quad VA=0
X⪰ΓT​Γ+VT​V.\displaystyle X\succeq\Gamma^{T}\Gamma+V^{T}V.
X∗−(Γ∗)T​Γ∗=(V∗)T​V∗\displaystyle X^{*}-(\Gamma^{*})^{T}\Gamma^{*}=(V^{*})^{T}V^{*}
(Γ∗)T​B\displaystyle(\Gamma^{*})^{T}B =X∗​A\displaystyle=X^{*}A
=(Γ∗)T​Γ∗​A+(V∗)T​V∗​A\displaystyle=(\Gamma^{*})^{T}\Gamma^{*}A+(V^{*})^{T}V^{*}A
=(Γ∗)T​B+(V∗)T​V∗​A,\displaystyle=(\Gamma^{*})^{T}B+(V^{*})^{T}V^{*}A,

-C Proofs for Symmetry Reduction

Orthogonal congruence preserves PSD slack, its block traces and the implied boundary equation. A block invariant under every orthogonal conjugation must be a scalar multiple of the identity, yielding the fixed-point form. Its eigenvalues repeat n times, so S⪰0 is equivalent to Λ⪰0. Substitution then recovers every proposed SDP constraint and its objective, proving the reduction without changing optimal cost.

-D Bézier Curves and the Bernstein Basis

The implementation uses standard Bernstein polynomials, restoring the binomial factors omitted in the simpler main derivation. Product and s(1−s) identities translate between degrees; all original identities are retained.

Equations 25–27
Bid​(s):=(di)​si​(1−s)d−i,\displaystyle B_{i}^{d}(s):=\binom{d}{i}s^{i}(1-s)^{d-i}, (25)
s⁡(1−s)​Bid​(s)\displaystyle s(1-s)B^{d}_{i}(s) =(d+1−i)​(i+1)(d+1)​(d+2)​Bi+1d+2​(s),\displaystyle=\frac{(d+1-i)(i+1)}{(d+1)(d+2)}B^{d+2}_{i+1}(s), (26)
Bid​(s)​Bjd​(s)\displaystyle B_{i}^{d}(s)B_{j}^{d}(s) =(di)​(dj)(2​di+j)​Bi+j2​d​(s).\displaystyle=\frac{\binom{d}{i}\binom{d}{j}}{\binom{2d}{i+j}}B^{2d}_{i+j}(s). (27)

-D 1 Curve Costs for Bézier Curves

Repeated forward differences and the factor d!/(d−k)! give Bézier derivatives. Integrating Bernstein products gives an analytic Gram matrix; sandwiching it by the difference operators and the squared factorial factor produces G⁽ᵏ⁾. This avoids numerical quadrature for the derivative objective.

Equations 28–30
γ(k)​(s)=d!(d−k)!​Γ​Dd(k)​bd−k​(s),\displaystyle\gamma^{(k)}(s)=\frac{d!}{(d-k)!}\,\Gamma D_{d}^{(k)}b_{d-k}(s), (28)
[Gd]i​j=12​d+1​(di)​(dj)(2​di+j),\displaystyle[G_{d}]_{ij}=\frac{1}{2d+1}\frac{\binom{d}{i}\binom{d}{j}}{\binom{2d}{i+j}}, (29)
G(k):=(d!(d−k)!)2​Dd(k)​Gd−k​(Dd(k))T.\displaystyle G^{(k)}:=\left(\frac{d!}{(d-k)!}\right)^{2}D_{d}^{(k)}\,G_{d-k}\,(D_{d}^{(k)})^{T}. (30)

-D 2 Collision Avoidance for Bézier Curves

Partition of unity makes u the all-ones vector. In standard Bernstein coordinates, antidiagonal sums gain ratios of binomial coefficients, and the shifted map gains k(2d−k)/(2d(2d−1)). These weighted maps implement the same exact interval collision certificate.

Equations 31–33
∑i=0dBid​(s)=eT​bd​(s)=1.\displaystyle\sum_{i=0}^{d}B_{i}^{d}(s)=e^{T}b_{d}(s)=1. (31)
[𝒮d​(M)]k=∑i+j=k(di)​(dj)(2​dk)​Mi​j.\displaystyle[\mathcal{S}_{d}(M)]_{k}=\sum_{\begin{subarray}{c}i+j=k\end{subarray}}\frac{\binom{d}{i}\binom{d}{j}}{\binom{2d}{k}}M_{ij}. (32)
[𝒯d​(M)]k={k⁡(2​d−k)2​d​(2​d−1)​[𝒮d−1​(M)]k−1if ​1≤k≤2​d−1,0otherwise.\displaystyle[\mathcal{T}_{d}(M)]_{k}=\begin{cases}\frac{k(2d-k)}{2d(2d-1)}[\mathcal{S}_{d-1}(M)]_{k-1}&\text{if }1\leq k\leq 2d-1,\\ 0&\text{otherwise}.\end{cases} (33)

Acknowledgement

Support is credited to NSF’s HAND engineering research center, three Amazon awards, and the Aker Scholarship. The original funding statement distinguishes the authors’ conclusions from those of the funders.

Concept demonstration

Open image ↗