arxiv: 2605.14199 · v1 · submitted 2026-05-13 · 💻 cs.RO · cs.SY· eess.SY

Recognition: 1 theorem link

· Lean Theorem

Motion Planning for Autonomous Vehicles using Optimization over Graphs of Convex Sets

Matheus Wagner , Ant\^onio Augusto Fr\"ohlich

Authors on Pith no claims yet

Pith reviewed 2026-05-15 04:42 UTC · model grok-4.3

classification 💻 cs.RO cs.SYeess.SY

keywords motion planningautonomous vehiclesgraphs of convex setstrajectory optimizationcollision avoidancedynamic feasibilityBezier curvesoptimal control

0 comments

The pith

Optimization over graphs of convex sets approximates nonlinear optimal control for autonomous vehicle motion planning with greater efficiency.

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The paper investigates using optimization over graphs of convex sets to generate collision-free and dynamically feasible trajectories for autonomous vehicles. It represents the environment as a directed graph of convex regions, allowing nonconvex geometry to be managed through connectivity decisions while keeping optimization convex within regions. Vehicle motion is modeled with Bezier curves for paths and polynomial time scaling, with dynamic constraints derived from a simplified bicycle model under linear tire assumptions. This yields solutions close to those of nonlinear programs but with better computational performance and less sensitivity to starting points in scenarios like obstacle avoidance and lane changes.

Core claim

The GCS-based method generates collision-free and dynamically consistent trajectories that closely match those obtained from the nonlinear program, while exhibiting improved computational efficiency and reduced sensitivity to initialization.

What carries the argument

Graphs of Convex Sets (GCS) organizing free space into a directed graph of convex regions, combined with Bezier curve parameterization of spatial paths and polynomial time-scaling, under convex constraints from a linear-tire bicycle model.

If this is right

Produces trajectories that satisfy collision avoidance and approximate dynamic feasibility in static obstacle and lane-change scenarios.
Offers a convex relaxation that captures dominant geometric and dynamic effects of full nonlinear formulations.
Reduces computational time compared to direct nonlinear optimal control.
Lowers dependence on accurate initial guesses for the solver.

Where Pith is reading between the lines

These are editorial extensions of the paper, not claims the author makes directly.

Extending the graph to include time-varying regions could handle moving obstacles.
Combining GCS with perception modules might enable online replanning in uncertain environments.
The convex structure could facilitate warm-starting or parallel computation in multi-vehicle settings.

Load-bearing premise

A simplified bicycle model with small-slip and linear tire assumptions provides sufficient convex constraints to enforce approximate dynamic feasibility.

What would settle it

Comparing trajectories in high-slip maneuvers, such as sharp turns at high speeds, where the GCS solutions deviate significantly from the nonlinear program or real vehicle behavior.

Figures

Figures reproduced from arXiv: 2605.14199 by Ant\^onio Augusto Fr\"ohlich, Matheus Wagner.

**Figure 1.** Figure 1: Trajectories obtained using (a) GCS and (b) NLP for the static obstacle avoidance scenario. The orange rectangles represent the occupancy of the ego [PITH_FULL_IMAGE:figures/full_fig_p007_1.png] view at source ↗

**Figure 2.** Figure 2: (a) Minimum distance from obstacles, (b) longitudinal velocity, (c) [PITH_FULL_IMAGE:figures/full_fig_p007_2.png] view at source ↗

**Figure 3.** Figure 3: Trajectories obtained using (a) GCS and (b) NLP for the lane changing scenario. The orange rectangles represent the occupancy of the ego vehicle; [PITH_FULL_IMAGE:figures/full_fig_p008_3.png] view at source ↗

**Figure 4.** Figure 4: (a) Minimum distance from obstacles, (b) longitudinal velocity, (c) [PITH_FULL_IMAGE:figures/full_fig_p008_4.png] view at source ↗

**Figure 5.** Figure 5: Trajectories obtained using (a) GCS and (b) NLP for the overtaking scenario. The orange rectangles represent the occupancy of the ego vehicle; the [PITH_FULL_IMAGE:figures/full_fig_p009_5.png] view at source ↗

**Figure 6.** Figure 6: (a) Minimum distance from obstacles, (b) longitudinal velocity, (c) [PITH_FULL_IMAGE:figures/full_fig_p009_6.png] view at source ↗

read the original abstract

Motion planning for autonomous vehicles requires generating collision-free and dynamically feasible trajectories in complex environments under real-time constraints. While nonlinear optimal control formulations provide high-fidelity solutions, they are computationally demanding and sensitive to initialization, whereas geometric planning methods scale well but often decouple path selection from trajectory optimization. This paper studies the extent to which optimization over Graphs of Convex Sets (GCS) can approximate solutions of nonlinear optimal control problems in the context of autonomous driving. The free space is represented as a finite union of convex regions organized as a directed graph, allowing nonconvex geometry to be handled through discrete connectivity decisions while maintaining convex trajectory constraints within each region. Vehicle motion is parameterized using Bezier curves for the spatial path and a polynomial time-scaling function for temporal evolution. Under small-slip and linear tire assumptions, a simplified dynamic bicycle model enables approximate enforcement of dynamic feasibility through convex constraints on trajectory derivatives. The approach is evaluated in CommonRoad scenarios involving static obstacle avoidance and lane-changing maneuvers, and is compared against a nonlinear discrete-time optimal control formulation. The results indicate that the GCS-based method generates collision-free and dynamically consistent trajectories that closely match those obtained from the nonlinear program, while exhibiting improved computational efficiency and reduced sensitivity to initialization. These findings suggest that GCS provides a structured approximation of nonlinear motion planning problems, capturing dominant geometric and dynamic effects while preserving convexity in the continuous relaxation.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit. Tearing a paper down is the easy half of reading it; the pith above is the substance, this is the friction.

Desk Editor's Note private letter to a colleague

GCS with Bezier paths and a convex bicycle model gives a fast, initialization-robust approximation to nonlinear AV planning on CommonRoad cases, but the dynamic match depends on small-slip linear-tire assumptions that may not hold in sharper maneuvers.

read the letter

The paper's core result is that optimization over Graphs of Convex Sets, using Bezier spatial curves and polynomial time scaling plus convex constraints from a simplified bicycle model, produces trajectories that closely track those from a nonlinear discrete-time OCP while solving faster and showing less dependence on initialization. They demonstrate this on CommonRoad static-obstacle and lane-change scenarios, with the free space decomposed into convex regions connected by a directed graph so that nonconvex geometry is handled by discrete choices while keeping the continuous problem convex inside each region. That combination is new for vehicle planning and the empirical side-by-side comparison is the strongest part of the work; the reported solve-time gains and trajectory similarity are concrete and directly relevant to real-time deployment questions. The modeling choices are clearly stated, and the central claim of approximate dynamic feasibility is supported under the stated assumptions. The main limitation is the small-slip linear-tire approximation used to obtain convex constraints on derivatives. In lane-change runs where lateral accelerations push slip angles outside the linear regime, those constraints no longer guarantee that the output satisfies the original nonlinear tire forces, so the observed match could partly reflect the shared simplification rather than full dynamic fidelity. A post-hoc check against a nonlinear tire model on the produced trajectories would have strengthened the dynamic-consistency claim. The paper is aimed at researchers in robotics and control who already work with convex relaxations or GCS methods and want a practical AV planning example. It is worth sending to peer review because the experiments are direct, the implementation details are reproducible in principle, and the efficiency results address a real bottleneck even if the modeling assumptions need tighter validation.

Referee Report

2 major / 2 minor

Summary. The paper claims that optimization over Graphs of Convex Sets (GCS) approximates nonlinear optimal control for autonomous vehicle motion planning. Free space is modeled as a directed graph of convex regions; trajectories are parameterized by Bezier curves for the path and a polynomial time-scaling function; under linear-tire and small-slip assumptions a simplified bicycle model yields convex constraints on derivatives. On CommonRoad static-obstacle and lane-change scenarios the GCS solutions are reported to produce collision-free trajectories that closely match those of a nonlinear discrete-time program while improving runtime and initialization robustness.

Significance. If the reported approximation quality holds under the stated modeling assumptions, the work supplies a structured convex relaxation that retains geometric nonconvexity handling via graph connectivity while preserving convexity inside regions. The direct empirical comparison against an independent nonlinear baseline on standard benchmarks, together with the explicit parameterization choices, constitutes a concrete strength that supports the central claim of practical utility for real-time planning.

major comments (2)

[§3.2] §3.2 (Dynamic Feasibility): the claim that GCS trajectories are 'dynamically consistent' with the nonlinear program rests on convex constraints derived from the linear-tire, small-slip bicycle model. In the evaluated lane-change cases, peak lateral accelerations can produce slip angles outside the linear regime; without a post-hoc nonlinear tire-model verification the observed match may be an artifact of the shared simplified dynamics rather than evidence of genuine approximation quality.
[§4.3] §4.3 and Table 2: the efficiency and robustness advantages are quantified only for the specific Bezier degree and convex-region decomposition chosen; the manuscript does not report sensitivity of these metrics to those free parameters, weakening the general claim that GCS is less sensitive to initialization than the nonlinear baseline.

minor comments (2)

[Figure 3] Figure 3: axis labels and legend entries for the GCS versus nonlinear trajectories are too small; increasing font size would improve readability of the overlay comparison.
Notation: the time-scaling polynomial is introduced without an explicit equation number; adding an equation label would clarify subsequent references to its coefficients.

Simulated Author's Rebuttal

2 responses · 0 unresolved

We thank the referee for the constructive and detailed comments, which help clarify the scope and strengthen the presentation of our results. We respond to each major comment below and indicate the planned revisions.

read point-by-point responses

Referee: [§3.2] §3.2 (Dynamic Feasibility): the claim that GCS trajectories are 'dynamically consistent' with the nonlinear program rests on convex constraints derived from the linear-tire, small-slip bicycle model. In the evaluated lane-change cases, peak lateral accelerations can produce slip angles outside the linear regime; without a post-hoc nonlinear tire-model verification the observed match may be an artifact of the shared simplified dynamics rather than evidence of genuine approximation quality.

Authors: We thank the referee for this observation. The nonlinear baseline is deliberately formulated with the identical simplified bicycle model (linear tire, small-slip assumptions) so that the comparison isolates the effect of the convex relaxation versus nonlinear optimization under the same modeling assumptions; the GCS method is explicitly presented as an approximation within this framework rather than a claim of fidelity to full nonlinear tire physics. To address the concern about regime validity, we will add a post-hoc verification step in the revised §3.2 that recomputes slip angles and lateral forces on the obtained GCS trajectories using a nonlinear (Pacejka) tire model. Any trajectories exceeding the linear regime will be flagged and discussed, with possible tightening of acceleration bounds or scenario adjustments if needed. This verification will be reported alongside the existing results. revision: yes
Referee: [§4.3] §4.3 and Table 2: the efficiency and robustness advantages are quantified only for the specific Bezier degree and convex-region decomposition chosen; the manuscript does not report sensitivity of these metrics to those free parameters, weakening the general claim that GCS is less sensitive to initialization than the nonlinear baseline.

Authors: We agree that explicit sensitivity analysis would strengthen the general claims. The chosen Bezier degree and region decomposition follow standard GCS practices for the CommonRoad scenarios, but we will expand §4.3 with additional experiments that vary the Bezier degree (5, 7, 9) and use both coarser and finer convex decompositions. For each combination we will report runtime, success rate, and initialization robustness (measured by convergence from random initial guesses) in an updated Table 2 and accompanying text. This will demonstrate that the reported advantages are not artifacts of a single parameter setting. revision: yes

Circularity Check

0 steps flagged

No significant circularity detected

full rationale

The paper derives GCS-based trajectories via Bezier parameterization and convex constraints from a linear tire/small-slip bicycle model, then directly compares them to an independent nonlinear optimal control formulation on CommonRoad scenarios. No central result reduces to a fitted parameter, self-defined quantity, or load-bearing self-citation; the modeling assumptions are stated explicitly and the benchmark is external. The derivation chain remains self-contained.

Axiom & Free-Parameter Ledger

2 free parameters · 2 axioms · 0 invented entities

The method rests on standard convex-optimization assumptions plus domain-specific simplifications of vehicle dynamics; no new entities are postulated.

free parameters (2)

Bezier curve degree
Chosen to trade off smoothness against the number of decision variables in the convex program.
Convex region decomposition
Environment-specific partitioning into convex sets whose count and connectivity affect both feasibility and solve time.

axioms (2)

domain assumption Free space can be expressed as a finite union of convex sets connected by a directed graph
Invoked to convert nonconvex geometry into discrete connectivity decisions while keeping intra-region constraints convex.
domain assumption Small-slip and linear tire model
Used to obtain convex constraints on trajectory derivatives from the bicycle model.

pith-pipeline@v0.9.0 · 5550 in / 1345 out tokens · 40024 ms · 2026-05-15T04:42:06.539061+00:00 · methodology

discussion (0)

Lean theorems connected to this paper

Citations machine-checked in the Pith Canon. Every link opens the source theorem in the public Lean library.

IndisputableMonolith/Foundation/RealityFromDistinction.lean reality_from_one_distinction unclear

?

unclear
Relation between the paper passage and the cited Recognition theorem.

Under small-slip and linear tire assumptions, a simplified dynamic bicycle model enables approximate enforcement of dynamic feasibility through convex constraints on trajectory derivatives.

What do these tags mean?

matches: The paper's claim is directly supported by a theorem in the formal canon.
supports: The theorem supports part of the paper's argument, but the paper may add assumptions or extra steps.
extends: The paper goes beyond the formal theorem; the theorem is a base layer rather than the whole result.
uses: The paper appears to rely on the theorem as machinery.
contradicts: The paper's claim conflicts with a theorem or certificate in the canon.
unclear: Pith found a possible connection, but the passage is too broad, indirect, or ambiguous to say the theorem truly supports the claim.

Reference graph

Works this paper leans on

26 extracted references · 26 canonical work pages

[1]

Model pre- dictive control for autonomous ground vehicles: a review,

S. Yu, M. Hirche, Y . Huang, H. Chen, and F. Allg ¨ower, “Model pre- dictive control for autonomous ground vehicles: a review,”Autonomous Intelligent Systems, vol. 1, 2021

work page 2021
[2]

Self- driving cars: A survey,

C. Badue, R. Guidolini, R. V . Carneiro, P. Azevedo, V . B. Cardoso, A. Forechi, L. Jesus, R. Berriel, T. M. Paixao, F. Mutzet al., “Self- driving cars: A survey,”Expert systems with applications, vol. 165, p. 113816, 2021

work page 2021
[3]

A review of learning-based motion planning: Toward a data-driven optimal control approach,

J. Hu, Y . Chang, and H. Wang, “A review of learning-based motion planning: Toward a data-driven optimal control approach,”ArXiv, vol. abs/2512.11944, 2025. [Online]. Available: https://api.semanticscholar. org/CorpusID:283895814

work page arXiv 2025
[4]

A survey of motion planning and control techniques for self-driving urban vehicles,

B. Paden, M. ˇC´ap, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Transactions on intelligent vehicles, vol. 1, no. 1, pp. 33–55, 2016

work page 2016
[5]

Spatio-temporal lattice planning using opti- mal motion primitives,

A. Botros and S. L. Smith, “Spatio-temporal lattice planning using opti- mal motion primitives,”IEEE Transactions on Intelligent Transportation Systems, vol. 24, no. 11, pp. 11 950–11 962, 2023

work page 2023
[6]

Motion planning for autonomous driving: The state of the art and future perspectives,

S. Teng, X. Hu, P. Deng, B. Li, Y . Li, Y . Ai, D. Yang, L. Li, Z. Xuanyuan, F. Zhuet al., “Motion planning for autonomous driving: The state of the art and future perspectives,”IEEE Transactions on Intelligent V ehicles, vol. 8, no. 6, pp. 3692–3711, 2023

work page 2023
[7]

Rapidly-exploring random trees: A new tool for path planning,

S. LaValle, “Rapidly-exploring random trees: A new tool for path planning,”Research Report 9811, 1998

work page 1998
[8]

Rdt-rrt: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles,

J. Yu, C. Chen, A. Arab, J. Yi, X. Pei, and X. Guo, “Rdt-rrt: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles,”Expert Systems with Applications, vol. 240, p. 122510, 2024

work page 2024
[9]

Sampling-based motion planning: A comparative review,

A. Orthey, C. Chamzas, and L. E. Kavraki, “Sampling-based motion planning: A comparative review,”Annual Review of Control, Robotics, and Autonomous Systems, vol. 7, 2023

work page 2023
[10]

Risk-aware self-consistent imitation learning for trajectory planning in autonomous driving,

Y . Fan, Y . Li, and S. Wang, “Risk-aware self-consistent imitation learning for trajectory planning in autonomous driving,” inEuropean Conference on Computer Vision. Springer, 2024, pp. 270–287

work page 2024
[11]

Learning hierarchical behavior and motion planning for autonomous driving,

J. Wang, Y . Wang, D. Zhang, Y . Yang, and R. Xiong, “Learning hierarchical behavior and motion planning for autonomous driving,” in2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 2235–2242

work page 2020
[12]

Diffusion-es: Gradient-free planning with diffusion for autonomous and instruction-guided driving,

B. Yang, H. Su, N. Gkanatsios, T.-W. Ke, A. Jain, J. Schneider, and K. Fragkiadaki, “Diffusion-es: Gradient-free planning with diffusion for autonomous and instruction-guided driving,” inProceedings of the IEEE/CVF conference on computer vision and pattern recognition, 2024, pp. 15 342–15 353

work page 2024
[13]

Diffeomorphism-transformed iterative linear quadratic regulator for constrained motion planning in autonomous driving,

Z. Zhu, H. Liu, W. Wang, J. Duan, H. Zhao, and J. Ma, “Diffeomorphism-transformed iterative linear quadratic regulator for constrained motion planning in autonomous driving,”IEEE Transactions on Intelligent Transportation Systems, vol. 26, no. 10, pp. 15 175–15 189, 2025

work page 2025
[14]

J. T. Betts,Practical methods for optimal control and estimation using nonlinear programming. SIAM, 2010

work page 2010
[15]

Nmpc tra- jectory planner for urban autonomous driving,

F. Micheli, M. Bersani, S. Arrigoni, F. Braghin, and F. Cheli, “Nmpc tra- jectory planner for urban autonomous driving,”V ehicle system dynamics, vol. 61, no. 5, pp. 1387–1409, 2023

work page 2023
[16]

Rti-nmpc for control of autonomous vehicles using implicit discretization methods,

M. Wagner and J. E. Normey-Rico, “Rti-nmpc for control of autonomous vehicles using implicit discretization methods,” inSimp ´osio Brasileiro de Automac ¸˜ao Inteligente-SBAI, vol. 1, no. 2, 2023

work page 2023
[17]

Quartic b ´ezier curve based trajectory generation for autonomous vehicles with curvature and velocity constraints,

C. Chen, Y . He, C. Bu, J. Han, and X. Zhang, “Quartic b ´ezier curve based trajectory generation for autonomous vehicles with curvature and velocity constraints,” in2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014, pp. 6108–6113

work page 2014
[18]

Local trajectory planning and tracking of autonomous vehicles, using clothoid tentacles method,

C. Alia, T. Gilles, T. Reine, and C. Ali, “Local trajectory planning and tracking of autonomous vehicles, using clothoid tentacles method,” in 2015 IEEE intelligent vehicles symposium (IV). IEEE, 2015, pp. 674– 679

work page 2015
[19]

Graph-theoretic b ´ezier curve optimization over safe corridors for safe and smooth motion planning,

S. Zayou and O. Arslan, “Graph-theoretic b ´ezier curve optimization over safe corridors for safe and smooth motion planning,” in2025 European Control Conference (ECC), 2025, pp. 1364–1371

work page 2025
[20]

Path planning on large curvature roads using driver-vehicle-road system based on the kinematic vehicle model,

J. Wang, Y . Yan, K. Zhang, Y . Chen, M. Cao, and G. Yin, “Path planning on large curvature roads using driver-vehicle-road system based on the kinematic vehicle model,”IEEE Transactions on V ehicular Technology, vol. 71, no. 1, pp. 311–325, 2021

work page 2021
[21]

Motion planning around obstacles with convex optimization,

T. Marcucci, M. Petersen, D. V on Wrangel, and R. Tedrake, “Motion planning around obstacles with convex optimization,”Science robotics, vol. 8, no. 84, p. eadf7843, 2023

work page 2023
[22]

A unified and scalable method for optimization over graphs of convex sets,

T. Marcucci, “A unified and scalable method for optimization over graphs of convex sets,”arXiv preprint arXiv:2510.20184, 2025

work page arXiv 2025
[23]

Rajamani,V ehicle dynamics and control

R. Rajamani,V ehicle dynamics and control. Springer Science & Business Media, 2011

work page 2011
[24]

Differential flatness of mechanical control systems: A catalog of prototype systems,

M. Rathinam, R. M. Murray, and W. M. Sluis, “Differential flatness of mechanical control systems: A catalog of prototype systems,” Dynamic Systems and Control: V olume 1 — Vibration Control; Dynamic Systems; Robotics; Sliding Mode Control; Robust and Nonlinear Control; Automated Modeling; Control of Manufacturing Processes; Precision Control, 1995. [Onlin...

work page 1995
[25]

Commonroad scenario designer: An open-source toolbox for map conversion and scenario creation for autonomous vehicles,

S. Maierhofer, M. Klischat, and M. Althoff, “Commonroad scenario designer: An open-source toolbox for map conversion and scenario creation for autonomous vehicles,” inProc. of the IEEE Int. Conf. on Intelligent Transportation Systems, 2021, pp. 3176–3182

work page 2021
[26]

Computing large convex regions of obstacle- free space through semidefinite programming,

R. Deits and R. Tedrake, “Computing large convex regions of obstacle- free space through semidefinite programming,” inAlgorithmic F ounda- tions of Robotics XI: Selected Contributions of the Eleventh International Workshop on the Algorithmic F oundations of Robotics. Springer, 2015, pp. 109–124

work page 2015