CAV Trajectory Planning Method

• CAV trajectory planning is a formal strategy for computing collision-free, dynamically feasible paths while satisfying vehicle kinematics, actuator, and safety constraints.
• The method uses spatial transformation and sequential linear programming with vehicle rectangle modeling to optimize curvature and maximize admissible speeds.
• It ensures high performance in both roomy and constrained environments by iteratively updating linearized vehicle geometry and maintaining actuator feasibility.

A connected and automated vehicle (CAV) trajectory planning method is a formal strategy for computing dynamically feasible, collision-free, and performance-optimized paths for CAVs while respecting all relevant geometric, actuator, and safety constraints. The trajectory planning problem encompasses the simultaneous satisfaction of vehicle kinematics, explicit vehicle dimension constraints, actuator bounds, road/obstacle boundaries, and curvature limits, and it often seeks to maximize admissible speeds or minimize some cost functional such as curvature, time, energy, or control effort. A central theoretical and practical contribution is a convex spatial-domain optimization framework built on sequential linear programming (SLP), which, when combined with accurate vehicle rectangle modeling and spatially varying constraints, achieves high-performance planning in both roomy and extremely constrained driving environments.

1. Spatial Transformation and Kinematic Modeling

Conventional time-based trajectory planning for CAVs requires dynamic models that depend on time as the independent variable, such as the nonlinear kinematic bicycle model:

$$\frac{dx}{dt} = v \cos \psi, \quad \frac{dy}{dt} = v \sin \psi, \quad \frac{d\psi}{dt} = \frac{v}{L} \tan \delta$$

where $x$, $y$ are global position, $\psi$ is the heading, $v$ is the (fixed or predetermined) velocity, $L$ is the wheelbase, and $\delta$ is steering.

To simplify the enforcement of spatially defined safety constraints (i.e., road envelope, obstacles), the problem is recast in a road-aligned (curvilinear) coordinate frame with $s$ as the longitudinal distance along a "road centerline" and $e_y$ as the lateral deviation. The planning variable shifts from time $t$ to the spatial variable $s$, yielding a set of dynamics for the vehicle's lateral/heading error:

• $s$ : along-centerline spatial coordinate (independent variable)
• $e_y$ : lateral deviation from centerline
• $e_\psi$ : heading error

Spatial dynamics are then derived by applying the chain rule and re-expressing the kinematic model in $(s, e_y, e_\psi)$ as a function of $s$, removing the need for explicit time representation and facilitating synchronization with spatially distributed obstacles and boundaries.

2. Vehicle Dimension Constraints: Local Linearization

Instead of representing the vehicle as a point mass or inflating obstacles by safety margins, this approach computes the positions of the actual four corners of the rectangular footprint in the $(s, e_y)$ frame for all planning states, using the geometric parameters:

$$\begin{align*} s_{c_i} &= s + \xi_{c_i}^s \cos e_\psi + \zeta_{c_i}^s \sin e_\psi \ e_{y,c_i} &= e_y + \xi_{c_i}^{e_y} \cos e_\psi + \zeta_{c_i}^{e_y} \sin e_\psi \end{align*}$$

Here, $(\xi_{c_i}^s, \zeta_{c_i}^s)$ and $(\xi_{c_i}^{e_y}, \zeta_{c_i}^{e_y})$ are coefficients defined by the vehicle's distances to its front/rear and lateral edges.

These constraints, being nonlinear due to their trigonometric dependence on $e_\psi$, are locally linearized about a reference path at each SLP iteration:

$$Q_j^\text{lower} \cdot z_j \geq q_j^\text{lower}, \qquad Q_j^\text{upper} \cdot z_j \leq q_j^\text{upper}$$

where $z_j$ gathers the sampled states at planning point $j$, and $Q_j$, $q_j$ encode the current grid, nominal trajectory, and geometry.

This linearization allows the actual vehicle rectangle, rather than an overinflated point, to be kept within free space defined by obstacle boundaries at each discretized step, resulting in less conservative and more efficient use of available road area, especially in tight environments.

3. Sequential Linear Programming (SLP) Algorithm

The CAV trajectory optimization is structured as a sequential linear programming routine:

1. Initialization: A reference trajectory for $(e_y, e_\psi, \delta)$ is created. This is typically a "warm start" from affine segment concatenations (e.g., clothoids) that respect basic corridor avoidance, with low overall heading change.
2. Linearization: Around each reference, both the spatial vehicle dynamics (in $(s, e_y)$) and the nonlinear dimension constraints are linearized.
3. Discretization: The spatial planning interval is discretized using an adaptive grid, with finer resolution around obstacles or sharp curve features.
4. LP Formulation: At each iteration, the algorithm sets up and solves a linear program with the objective:

$$\min \max|u| + \lambda \cdot \max|D_1 u| + W_\sigma \left( \sigma + \sigma_{e_y}^N + \sigma_{e_\psi}^N \right)$$

Here, $u$ are the steering commands (curvature inputs), $D_1 u$ is the steering rate difference (for smoothness), and the $\sigma$ variables are slack terms for constraint relaxation with a large penalty $W_\sigma$ to ensure feasibility.

Constraints include: - Discretized spatial vehicle kinematics - Actuator and rate limits (box constraints on $\delta$ and $\Delta \delta$) - Obstacle/corridor boundaries - Linearized geometry (vehicle corner) constraints

1. Update and Iteration: The solution updates the reference trajectory. If any constraint is violated (obstacles encroached, or trajectory too jagged), or if the stopping criterion is not yet met, the linearization and LP solution are repeated.
2. Termination: Iterations continue until convergence (solution change below threshold) or maximum iterations.

This SLP framework ensures convergence to a trajectory that both respects hard geometric and actuator constraints and globally considers the entire spatial constellation of obstacles.

4. Objective Formulation and Admissible Speed Bounds

The LP objective function structure prioritizes minimizing the peak curvature along the path, since high curvature segments reduce the maximum vehicle speed admissible due to tire friction limits. For a given tire-road friction coefficient $\mu$, the maximum physically safe traversable speed at each path point, $v^\text{max,fric}(\eta)$, is set by

$v_\text{ref}(\eta) \leq v^{\text{max,fric}}(\eta)$

where $v^\text{max,fric}$ is a function of local curvature and physical vehicle/tire parameters.

By minimizing the max absolute curvature and post-computing $v^\text{max,fric}(\eta)$, the method produces trajectories that permit higher allowable speeds without exceeding friction or actuator bounds, directly embedding performance considerations within the planning LP.

5. Performance in Constrained and Roomy Scenarios

The method is validated in two differing scenarios:

• Roomy corridor: With well-separated obstacles and moderate curvature, SLP and clothoid-based reference methods produce obstacle-avoiding trajectories. However, SLP yields paths that saturate corridor boundaries and minimize curvature, supporting higher speed bounds than rival semi-analytical clothoid planners, which result in more conservative, lower-speed paths.
• Tight environment (e.g., parking): The SLP formulation (without overinflating obstacles) ensures that actuator limits are never exceeded and that the trajectory robustly remains feasible despite constrained geometry. The approach quickly converges to a smooth, collision-free path within a handful of SLP iterations.

The following table summarizes qualitative differences observed in the two scenarios:

Property	SLP-based (This Method)	Clothoid-based (CPP)
Curvature Peaks	Lower	Higher
Speed Bound Util.	Maximized (saturates boundaries)	Conservative (unused road margin)
Actuator Feasibility	Preserved	Sometimes violated (tight spaces)
Convergence	Rapid (few iterations)	N/A (closed-form/semi-analytic)

The vehicle geometry, modeled exactly by rectangle constraints, enables computation of efficient trajectories in spaces where point-mass or safety-margin methods would fail or yield overly cautious solutions.

6. Implementation Considerations and Limitations

Computational Requirements

• Each SLP iteration requires the solution of a convex LP, whose size depends on the number of discretization points and constraints.
• Linearizations and adaptive grid refinement contribute to computational load but are manageable due to LP efficiency.
• The method can be implemented on standard CPUs for real-time or near-real-time planning in practical scenarios.

Limitations and Applicability

• The method assumes the environment is well-modeled spatially, with accurate knowledge of road centerlines, boundaries, and obstacle envelopes.
• The accuracy of linearized vehicle dimension constraints depends on the validity of the reference trajectory. Large deviations between iterates can require additional SLP iterations for convergence.
• It is best suited to scenarios where explicit modeling of vehicle geometry is necessary, such as parking, narrow lanes, or densely obstructed roadways.

Deployment Strategy

• The approach is compatible with both online (receding horizon/rolling planning) and offline (pre-computed path) deployment, and is particularly effective as a path generator for local tracking controllers.
• Post-processing is required to recover (x, y, $\psi$, $\delta$, etc.) from the road-aligned spatial variables $(s, e_y, e_\psi)$.
• The algorithm explicitly avoids the need to "inflate" obstacles, supporting minimal safety margins in constrained environments.

7. Significance for CAV Systems

By formulating the CAV trajectory planning problem as an SLP over a transformed spatial kinematic model, with precise vehicle dimension constraints, the method enables:

• Maximized use of available free space, especially in tight or cluttered environments.
• Higher admissible vehicle speeds within actuator and friction limitations through direct curvature minimization.
• Trajectories that are both smooth and feasible under strict physical and geometric constraints.

This strategy is particularly compelling for autonomous driving in scenarios where traditional point-mass or margin-based methods are too conservative, and is foundational for the development of high-performance, constraint-saturated CAV path planning algorithms.