Dynamics-Aware Adaptive Corridors with Feasibility-Perturbed Trust-Region SQP for Certified Nonholonomic Motion Planning

Yang Shi

cs.ROOct 5, 2026

Dynamics-Aware Adaptive Corridors with Feasibility-Perturbed Trust-Region SQP for Certified Nonholonomic Motion Planning

Authors: Yang Shi

Organizations: Jiangsu College of Safety Technology, Jiangsu, China

Abstract

Optimisation-based parking planners usually impose collision constraints only at the time samples, so a vehicle corner can cut an obstacle between samples, and no executable trajectory exists until the solver converges. We present a planner for car-like vehicles with reverse gear in which every iterate of the optimisation phase satisfies the discretised dynamics exactly and keeps the whole vehicle rectangle clear of obstacles between the samples. Each time interval receives one convex corridor that holds all vehicle corners at both ends and is shrunk by a sweep margin bounding how far the corner paths leave their chords. Separating half-planes give the corridors a direction out of obstacles when the initial guess is in collision; later, heading-aligned boxes are grown from the speed, curvature and step of the current iterate and rebuilt after accepted steps. A feasibility-perturbed trust-region sequential quadratic programming method projects each step onto the dynamics by feedback and verifies it exactly; the cost decreases monotonically, and once the corridors stop changing, limit points are Karush-Kuhn-Tucker points of the corridor-constrained problem or violate a constraint qualification. On 820 benchmark cases the planner succeeds in 818 without penetration (797 from the first initial guess), none of the 7103 evaluated optimisation-phase iterates is unusable, and it succeeds in 96.5% of the cases when 99% of the initial guesses intersect an obstacle. Its maneuvers take 0.7% longer in the median than those of a similarly certified exact-collision baseline. The guarantees hold for the planning model, not for a physical vehicle.

Figures & tables

Planner	Body model	Convex set and its update	Inter-sample	Usable iterates	Convergence result
OBCA, H-OBCA [ 4 , 5 ]	rectangle	none (exact dual constraints)	samples	–	local solver
STC [ 7 ]	two discs	one box per disc and sample, built once around the path	samples	–	local solver
LIOM [ 8 ]	two discs	boxes rebuilt from each intermediate solution	samples	– (dynamics penalised)	–
Embodied Footprints [ 22 ]	rectangle	none; footprint buffers as decision variables	buffers from speed and curvature	–	local solver
Chen et al. [ 17 ]	rectangle	obstacle subset selected by a trust region	samples	–	–
STO [ 16 ]	rectangle	polygon per sample, rebuilt at every SQP iteration	samples	–	–

TABLE I: Optimisation back ends for low-speed planning with obstacles, compared by the properties this paper targets. “Samples”: collision constraints hold only at the samples. “Usable iterates”: every iterate of the optimisation can be executed without collision and satisfies the dynamics. A dash means that the property is not claimed; “local solver”: convergence is that of the nonlinear programming solver used.

Panel of Figure 2 — Fig. 1: Structure of DA-ASC/FP-SQP. Phase I restores feasibility on SH polygons; in Phase II every accepted iterate is dynamically exact and collision-free between samples, so the loop can be interrupted after any accepted step.

Fig. 2: Corridors of one interval. (a) A DA-ASC box

\mathcal{P}_{k}

(shaded) grown from the seed box (dashed) holds all four corners at

k

and

k+1

; each corner moves along an arc (solid) that stays within

\varepsilon_{k,j}

of its chord (dotted), shown for the front-left corner. The step is 1.0 s here to make the arcs visible; the planner’s steps do not exceed 0.31 s. (b) An SH polygon (shaded) during Phase I: the bounding box

B_{k}

(dashed) is cut by one half-plane per obstacle. The vehicle overlaps the upper obstacle, so the corresponding row is violated and its normal gives the direction in which Phase I moves the vehicle.

Vehicle	$L=2.8$ m, $L_{\mathrm{f}}=0.96$ m, $L_{\mathrm{r}}=0.929$ m, $W=1.942$ m
Bounds	$\|v\|\leq 2.5$ m/s, $\|\delta\|\leq 0.75$ rad, $\|a\|\leq 1$ m/s 2 , $\|\omega\|\leq 0.5$ rad/s
Objective	$w_{T}=1$ , $(w_{a},w_{\omega})=(1,4)$ , $\sigma\in[0.2,3]$
Terminal box	0.1 m, 0.1 m, 0.05 rad, 0.05 m/s; $\delta$ free
Warm starts	$(v_{\mathrm{ref}},\text{time factor},h_{0})$ = $(0.8,1.5,0.4)$ , $(0.6,1.8,0.3)$ , $(0.5,2.2,0.25)$ [m/s, –, s], tried in this order; $N\leq 320$
Corridors	$\mu=0.02$ m, $l_{\max}=4$ m, $R_{\mathrm{SH}}=3$ m, $r_{\mathrm{pos}}=0.5$ m, $s_{\mathrm{act}}=0.1$ m, $s_{\mathrm{gain}}=0.02$ m, $B=5$
Phase I	$\mathcal{V}<10^{-7}$ , $\leq 100$ iterations, $\leq 2$ restarts

TABLE II: Vehicle and planner parameters used in all experiments

Method	820 cases		success per family [%]					time [s]		$T$ ratio
	success	pen.	perp.	par.	ang.	clut.	TPCAP	med.	P90	med. (pairs)
DA-ASC/FP-SQP (ours)	818	0	100	100	100	99.5	95.0	5.8	26.9	–
fixed corridors	815	0	100	100	100	98.0	95.0	4.9	21.8	0.997 (815)
OBCA-CT, 60 s	791	0	100	97.5	100	91.0	70.0	6.0	22.6	1.007 (791)
OBCA-CT, 3600 s	809	0	100	98.0	100	98.0	85.0	6.1	27.0	1.007 (808)
STC-IPOPT	786	16	95.5	100	100	90.5	70.0	1.7	3.1	0.998 (786)

TABLE III: Main comparison on 820 cases. Success requires the independent check to confirm a usable trajectory; “pen.” counts returned trajectories whose rectangle overlaps an obstacle. Times are optimisation times with eight runs in parallel. The

T

ratio is

T

(ours)/

T

(method), paired over the cases both solve.

Fig. 3: Main comparison. (a) Success rate per scenario family (200 cases each, TPCAP 20). (b) Paired ratio

T

(ours)/

T

(method) on the cases both methods solve (boxes: quartiles; whiskers: 10th and 90th percentiles); values below one mean that our manoeuvre is shorter. Our planner has the highest success rate, or shares it, in every family; the manoeuvres of OBCA-CT are 0.7% shorter in the median.

Fig. 4: Trajectories of four planners in three cases (vehicle outlines at every few samples; dashed: reverse gear; red: overlap with an obstacle found by the independent check). Parallel 10: OBCA with

d_{\min}=0.10

m clips the parked car between two samples. Cluttered 45: along almost the same path, the manoeuvre takes 29.6 s with fixed corridors and 21.7 s with rebuilding; STC-IPOPT penetrates an obstacle. TPCAP 15: rebuilding shortens the manoeuvre from 20.5 to 16.2 s; STC-IPOPT finds no solution.

Fig. 5: Interrupting the planners (139 cases). (a) Share of cases whose current iterate is usable if the run is stopped at wall time

t

; (b) share whose current iterate collides; (c) as (a) against normalised progress. None of the evaluated Phase-II iterates of our planner is unusable.

Fig. 6: Median time of one Phase-II QP and of one whole Phase-II iteration against the number of intervals

N

(eight cases, 15 iterations each, one thread). HPIPM grows linearly with

N

; Clarabel on the same QP is five to six times slower.

Variant	success	pen.	min. clear. [cm]	$T$ ratio	iterations
full planner	200/200	0	2.43	–	71
no rebuild (fixed corridors)	200/200	0	2.48	0.996	61
axis-aligned boxes	200/200	0	2.06	0.994	77
no sweep margin	200/200	0	1.78	1.000	66
no clearance buffer ( $\mu=0$ )	200/200	0	0.44	1.001	73
no sweep margin, $\mu=0$	194/200	6	$<0$	1.002	70

TABLE IV: Ablation on 200 cases (seeds 0–49 of the four random families). Clearance: smallest exact clearance over all returned trajectories.

T

ratio:

T

(full)/

T

(variant), paired median. Iterations: median Phase-II iterations.

Fig. 7: OBCA-CT with the sweep margin switched off: penetration depth of every penetrating interval against its margin

\varepsilon_{k}

(343 intervals, two grids). All points lie below the bound of Proposition 1 ; with the margin switched on, none of the 479 trajectories penetrates.

Fig. 8: Success rate when the front-end path is perturbed laterally (99 cases, two draws per amplitude). The share of perturbed warm starts that intersect an obstacle is given in brackets. Our planner keeps 96.5% at 1.0 m, where 99% of the warm starts are in collision.

Mismatch	cross-track [cm]		$e_{\theta}$ [deg]	clearance loss [cm]		$\mu=2$	$\mu=5$	$\mu=10$	$\mu=2$ , 90%	$\mu=10$ , 90%
	P95	max	max	P95	max	pen.	pen.	pen.	pen.	pen.
none	0.0	0.0	0.0	0.0	0.0	0	0	0	0	0
wheelbase $-5\%$	3.8	4.8	1.2	3.4	3.6	2	0	0	1	0
wheelbase $+5\%$	13.4	19.8	5.0	6.3	32.3	8	4	1	7	1
delay 50 ms	1.4	6.7	1.4	1.1	3.1	1	0	0	0	0
delay 100 ms	3.7	16.3	3.6	2.9	10.0	5	1	0	2	0

TABLE V: Closed-loop simulation on 219 cases. Error columns: plans with

\mu=2

cm; cross-track and clearance loss (planned minus realised clearance) as 95th percentile and maximum over runs. “pen.”: cases with a penetration in at least one run, for plans with the buffer

\mu

[cm] and, where marked, input bounds reduced to 90%.

Explore similar work

Sampling-based Certified Planning with Graphs of Convex Sets
Aug 30, 2026Peng Xie, Amr AlanwarCollision-Free Trajectories Convex Sets
TUM School of Computation, Information and Technology, Department of Computer Engineering, Technical University of Munich, 74076 Heilbronn, Germany
Model Predictive Planner for UAV Navigation in Non-Convex Air Corridors
Jul 27, 2026Henrique Silva, Marcelo A. Santos, Guilherme V. RaffoUnmanned Aerial Vehicle Trajectories Path Planning
Graduate Program in Electrical Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil · Department of Management, Information and Production Engineering, University of Bergamo, Dalmine, BG, Italy · Department of Electronic Engineering, Universidade Federal de Minas Gerais, Belo Horizonte, MG, Brazil
Semidefinite Relaxations for Collision-Free Motion Planning
Jun 12, 2026Bernhard Paus Graesdal, Alexandre Amice, Pablo A. Parrilo +1Collision-Free Trajectories Convex Relaxation
Department of Electrical Engineering and Computer Science, Massachusetts Institute of Technology, Cambridge, MA, USA.

Aug 30, 2026, cs.ROJ/K move · Enter open · S save

Sampling-based Certified Planning with Graphs of Convex Sets

Peng Xie, Amr Alanwar

TUM School of Computation, Information and Technology, Department of Computer Engineering, Technical University of Munich, 74076 Heilbronn, Germany

Collision-Free Trajectories Convex Sets Planning

Open paper AskPDF arXiv

Planners on graphs of convex sets return trajectories that are collision-free by construction, provided the convex regions are collision-free. The region generator only promises that property probabilistically, and no planner in the family verifies it. We report the first measurement of what the gap costs. On a scaled 14-DOF bimanual library,

3.2\%

of interface samples are in collision, and a search-based GCS planner (\gcsstar) turns that volume error into a

62\%

answer error:

18

29

pick-and-place queries return trajectories that drive the arms through the shelves, up to

91

,mm deep, reported as successes. Repairing the library does not work; a ten times stricter acceptance contract, sums-of-squares certified regions, and uniform margins each destroy the connectivity planning needs before they deliver soundness. We instead build a planner that certifies its answers. It samples the overlaps and shared faces of the decomposition, prunes with an admissible informed bound, and verifies the one candidate each search round proposes, continuously, by a chain of clearance certificate balls with no resolution parameter; failures are repaired with local in-region detours, and the convex polish is re-verified. Head-to-head on all

29

task queries it delivers zero invalid answers against

21

for the reference, reaches its first certified answer in

0.11

,s against

1.59

,s for the reference's unverified one, and reproduces the reference optimum exactly on every query whose reference answer is physically valid.

Abstract

Explore similar work

Sampling-based Certified Planning with Graphs of Convex Sets

Model Predictive Planner for UAV Navigation in Non-Convex Air Corridors

Semidefinite Relaxations for Collision-Free Motion Planning