Trajectory-Curvature Optimization

Updated 14 December 2025

Trajectory-curvature optimization is a framework that computes paths by incorporating curvature, curvature rate, and continuity constraints for smooth, kinodynamically feasible trajectories.
It employs methodologies such as spline parameterizations, convex approximations, and hierarchical solvers to balance constraints like collision avoidance and actuator feasibility.
The approach enables practical applications including real-time autonomous driving, precise robotic planning, and enhanced generative modeling through curvature-informed velocity profiling.

Trajectory-curvature optimization encompasses mathematical and algorithmic frameworks for computing dynamic or geometric trajectories that explicitly incorporate curvature, curvature rates, or higher-order continuity constraints. These approaches ensure kinodynamic feasibility, collision avoidance, comfort, maneuver efficiency, or computational tractability in domains including autonomous driving, racing, robotic planning, generative modeling, and model merging. Central to trajectory-curvature optimization is the simultaneous treatment of positional, orientation, and curvature variables, frequently using splines, piecewise curves, or direct optimal control methods, and enforcing bounds or penalizing total or peak curvature (and its derivatives). The subject encompasses hierarchical solvers, convex approximations, incremental gradient-based techniques, curvature-informed cost functional design, and dynamic mapping between curvature and velocity or control variables.

1. Mathematical Formulations and Geometric Fundamentals

Trajectory-curvature optimization often starts with a parameterization of the path using splines, polynomial segments, or analytical forms of motion primitives. For planar or spatial curves, curvature is defined as $\kappa(s) = \frac{x'(s)y''(s) - y'(s)x''(s)}{(x'(s)^2 + y'(s)^2)^{3/2}}$ for position functions $(x(s), y(s))$ . Sharpness is the derivative of curvature with respect to arc-length: $\sigma(s) = \frac{d\kappa}{ds}$ (Oliveira et al., 2018). Most frameworks require at least $C^1$ or $G^1$ (curvature continuity), with $C^2$ (sharpness) continuity favored for comfort and actuator feasibility in car-like vehicles.

Geometric constructions include piecewise circular-arcs (e.g., Dubins-type paths), planar-elastica segments (for trailer systems), and B-spline parameterizations for high-order smoothness and locality of control (Rao et al., 2024, Xue et al., 2023). Segment concatenation imposes junction-conditions for position, tangent, and sometimes curvature continuity.

2. Optimization Problem Structure and Constraint Handling

Integral objective functions combine arc-length, squared curvature, sharpness, and kinodynamic penalties:

Path smoothness: $\int \kappa(s)^2\, ds$ , total length, or control effort (e.g., $\sum \alpha_i^2$ for angular acceleration).
Feasibility: velocity, acceleration, curvature, and steering rate constraints, e.g., $|\kappa(s)| \leq \kappa_{\max}$ , $|a(t)| \leq a_{\max}$ , and oftentimes sharper bounds $|\sigma(s)| \leq \sigma_{\max}$ .
Collision avoidance: constraints on minimum distance to static/dynamic obstacles, frequently linearized in decision variables for convex-QP formulation (Babu et al., 2017, Yu et al., 7 Apr 2025).
Boundary constraints: initial/final pose, initial/final curvature, or tangent direction requirements.

Hierarchical or alternating minimization structures decouple high-dimensional nonlinear programs into tractable subproblems (e.g., angular acceleration vs. linear velocity layers) (Babu et al., 2017). Soft penalties rather than hard constraints are favored for kinodynamic and swept-volume constraints in spline-based approaches to enable smooth gradient-based optimization (Choi et al., 2023).

3. Numerical Methods and Computational Strategies

Representation choices (B-splines, quintic polynomials, piecewise cubics) allow a dramatic reduction in decision-variable dimension compared to pointwise discretizations (Xue et al., 2023). Convex quadratic programs arise in minimum curvature settings, with active-set solvers (qpOASES, HPIPM), interior point methods (IPOPT), or sequential quadratic programming (SQP) iterations achieving millisecond-scale solve times for hundreds of variables.

Collision constraints are managed via difference-of-convex (DC) relaxations, disc-type swept-volume estimation, local convex corridor generation (via GJK or ellipse-based shrink/expand methods), and incremental path flattening, where curvature weights are locally increased only in colliding regions (Choi et al., 2023, Yu et al., 7 Apr 2025). For parking problems, permitted curvature discontinuities at segment switches yield more efficient maneuvers (e.g., 6% shorter total length) than enforcing global curvature continuity (Yu et al., 7 Apr 2025).

The following table summarizes representative numerical frameworks:

Approach	Decision Variables	Core Solver
B-spline minimum curvature	Control point coordinates	qpOASES
Hierarchical alternating	$\{\alpha_i, v_i\}$ (angle/vel)	SQP (CVX)
STO with convex corridors	States/controls per segment	HPIPM SQP
IPF with SV/kinodynamics	Spline points, local weights	L-BFGS

4. Velocity Profile and Curvature-Velocity Coupling

Velocity profiling is closely tied to curvature for kinodynamic safety. Constraints such as $a_c(t) = v(t)^2\kappa(s(t)) \leq a_{c,\max}$ ensure that speed is reduced in high-curvature segments (tight turns), well captured in the two-phase methods where a $C^2$ guide-line is followed by time-optimal profile computation under centripetal and longitudinal acceleration bounds (Zhang et al., 2021).

Curvature-informed local trajectory planning is essential in racing: e.g., CiMPCC maps normalized curvature to a reference velocity $\tilde{v}(s)$ and penalizes deviations $|v(s) - v_\mathrm{ref}(s)|^2$ in the MPC objective (Li et al., 6 Feb 2025). This approach yielded 11.8% lap-time reductions over standard MPCC on real hardware.

5. Applications: Autonomous Vehicles, Robotics, Generative Modeling

Autonomous Driving and Racing

Trajectory-curvature optimization supports real-time path planning and smooth control for nonholonomic vehicles, including handling dynamic obstacles, executing lane changes, overtaking, and merging (Babu et al., 2017, Xue et al., 2023, Li et al., 6 Feb 2025). Parking maneuvers and unstructured environments benefit from segmented plans with allowed curvature discontinuities for maneuver efficiency, and safety corridor construction for collision avoidance (Yu et al., 7 Apr 2025).

Sharpness-continuous paths are necessary for heavy-duty vehicle control, directly handling steering rate and torque limitations for comfort and trackability (Oliveira et al., 2018).

Generative Modeling

Curvature minimization in ODE/SDE-based generative models directly affects numerical solver efficiency: lower curvature trajectories require fewer function evaluations for a desired error bound, reducing ODE truncation errors and sampling latency. Simulation-free curvature regularization is achievable via tailored loss terms in training, without explicit ODE simulation (Lee et al., 2023).

Model Merging

In neural model weight-space, curvature proxies (e.g., diagonal Hessian/Fisher traced from Adam second moments) enable robust merging of capability-specific fine-tuned models. Saliency-based sparsification (Fast Fisher Grafting) and curvature-aware aggregation suppress destructive interference, scaling to multi-skill LLMs with minimal overhead and negligible drop in performance (Mahdavinia et al., 14 Sep 2025).

6. Piecewise Geometric and Analytical Solutions

Curvature-constrained shortest or time-optimal paths are analytically characterized by concatenations of arcs, straight segments, and planar elastica (merging curves). For Dubins vehicles, three-arc constructs (CCC) cover the maximal set of reachable lengths, and at most two curvature discontinuity points are necessary (Rao et al., 2024).

For car-trailer systems, Pontryagin's Maximum Principle yields bang-bang, straight, and merging elastica (planar oscillatory curvature) segments; the latter form the smoothing interface between maximum curvature turns and straight segments (Chitsaz, 2013). In each case, algebraic equations (hyperbola loci, tangent continuity, adjoint boundary constraints) are solved to determine segment lengths and switching points.

7. Experimental Validation, Performance, and Scalability

Recent frameworks achieve real-time, robust, and experimentally validated performance. B-spline/IPF methods run in 60–300 ms per plan and yield $>96\%$ success in complex environments, outperforming state-of-the-art baselines (Choi et al., 2023). Alternating optimization offers a 50% speedup (e.g., 3.4 s vs 5.2 s per trajectory), with negligible loss in smoothness or arc-length (Babu et al., 2017). Parking scenarios confirm that permitted curvature jumps at gear-shifts yield maneuvers up to 6% shorter in length, and total solve times under 7 ms (Yu et al., 7 Apr 2025).

ODE-based generative models trained with curvature minimization achieve 2–5× faster sampling for equal output quality (verified by FID), with less distillation error and lower truncation error for neural solvers (Lee et al., 2023). Curvature-informed model merging in LLMs delivers higher merged-task scores and substantial memory savings (Mahdavinia et al., 14 Sep 2025).

Trajectory-curvature optimization thus provides foundational mathematical, algorithmic, and practical constructs for trajectory generation and model combination under curvature and kinodynamic constraints. It is validated across domains from autonomous vehicles and robotics to generative modeling and neural model merging, demonstrating both theoretical breadth and real-world impact.