Source-linked AI summary
An Efficient Spatial-Temporal Trajectory Planner for Autonomous Vehicles in Unstructured Environments
Zhichao Han, Yuwei Wu, Tong Li, Lu Zhang, Liuao Pei, Long Xu, Chengyang Li, Changjia Ma, Chao Xu, Shaojie Shen, Fei Gao
TL;DR
Real-time spatial-temporal trajectory optimization is challenging when nonholonomic dynamics and unstructured, dynamic obstacles must be handled together. The paper uses flat-space trajectory parameterization and obstacle-specific constraints, and reports benchmark advantages in efficiency and trajectory quality alongside real-world validation.
Problem
Real-time spatial-temporal planning must handle nonholonomic vehicle dynamics and full-dimensional obstacle avoidance while balancing trajectory quality and computation in unstructured environments.
Source: §I · pp. 1–2Method
The planner uses differential flatness to parameterize trajectories and analytically formulate constraints, with a safe driving corridor for static obstacles and signed-distance constraints for moving objects.
Source: §I · p. 2, B. Differentially Flat Vehicle Model · p. 3, B. Static Obstacle Avoidance · p. 5Results
Benchmark results show superiority in time efficiency and trajectory quality, with simulations reporting an order-of-magnitude efficiency advantage over other algorithms.
Source: A. Simulations · p. 10, §VIII · p. 13Takeaways & Limitations
Real-world experiments validate the planner on a real platform, supporting its practicality within the tested setting.
Source: §VIII · p. 13
Abstract
from arXiv · showhide
As a core part of autonomous driving systems, motion planning has received extensive attention from academia and industry. However, real-time trajectory planning capable of spatial-temporal joint optimization is challenged by nonholonomic dynamics, particularly in the presence of unstructured environments and dynamic obstacles. To bridge the gap, we propose a real-time trajectory optimization method that can generate a high-quality whole-body trajectory under arbitrary environmental constraints. By leveraging the differential flatness property of car-like robots, we simplify the trajectory representation and analytically formulate the planning problem while maintaining the feasibility of the nonholonomic dynamics. Moreover, we achieve efficient obstacle avoidance with a safe driving corridor for unmodelled obstacles and signed distance approximations for dynamic moving objects. We present comprehensive benchmarks with State-of-the-Art methods, demonstrating the significance of the proposed method in terms of efficiency and trajectory quality. Real-world experiments verify the practicality of our algorithm. We will release our codes for the research community
I. INTRODUCTION
The introduction frames real-time, high-quality motion planning in unstructured, dynamic environments as difficult because vehicle dynamics, full-shape collision avoidance, and spatial-temporal optimization create competing demands. The paper proposes flat-space trajectory optimization with analytically derived constraints and obstacle-specific safety modeling, and positions it against prior planning and collision-avoidance approaches.
I. INTRODUCTION
- Nonholonomic dynamics, full-dimensional obstacle avoidance, and spatial-temporal optimization make it difficult to produce feasible, high-quality trajectories online in complex environments.The introduction notes that rough vehicle-shape approximations can be conservative, while accurate modeling increases computation; joint space-time optimization can improve trajectory optimality but complicates optimization and real-time performance.Source: §I · pp. 1–2
- The proposed planner parameterizes trajectories in flat space and analytically derives continuously differentiable feasibility constraints for efficient optimization.It also reformulates the original optimization to be efficiently solvable without sacrificing optimality.Source: §I · p. 2
A. Trajectory Generations for Vehicles
- Sampling, decoupled path-and-dynamics methods, and discrete-state MPC each face stated tradeoffs in computation, trajectory quality, feasibility, or complex-environment performance.The cited approaches include sampling-based planners –, CES, DL-IAPS, and MPC-based methods –.Source: A. Trajectory Generations for Vehicles · p. 2
B. Obstacle Avoidance Formulation
- Existing obstacle-avoidance formulations can be conservative or costly: point-based models may restrict the solution space, while OBCA-based methods add obstacle-dependent dual variables.The introduction contrasts these limitations with the proposed spatial-temporal joint optimization and full-dimensional avoidance in unstructured environments.Source: B. Obstacle Avoidance Formulation · p. 3
III. SPATIAL-TEMPORAL TRAJECTORY PLANNING
- The planning formulation combines a front-end initial guess, a differentially flat vehicle model, and optimization over human comfort, execution time, and feasibility constraints.The paper identifies gradient propagation as part of the formulation for subsequent numerical optimization.Source: §III · p. 3
A. Planning Pipeline
The planner uses a lightweight search front end to initialize back-end optimization, representing car-like vehicle motion through flat outputs and finite derivatives. Its piecewise-polynomial trajectory optimization includes continuity, boundary, time, comfort, feasibility, and obstacle constraints.
A. Planning Pipeline
- A lightweight hybridA* front end supplies a collision-free initial path for back-end optimization, with Reed Shepp shooting used to support earlier search termination.For tasks involving forward and reverse motion, the front end determines segment directions and the trajectory is parameterized piecewise, assuming a complete stop at gear shifts.Source: A. Planning Pipeline · p. 3
B. Differentially Flat Vehicle Model
- The kinematic bicycle model uses rear-wheel-center position as its flat output, whose finite derivatives characterize vehicle state quantities and simplify trajectory planning.The motion direction is represented by η; velocity magnitude is fixed to a small non-zero constant at gear shifts to avoid singularities.Source: B. Differentially Flat Vehicle Model · p. 3
C. Optimization Formulation
- The optimization represents each trajectory segment with piecewise polynomials and minimizes control effort plus a duration penalty subject to boundary, continuity, feasibility, and obstacle constraints.The objective minimizes jerk for comfort; feasibility includes velocity, acceleration, and curvature, while obstacle terms cover static and dynamic avoidance.Source: C. Optimization Formulation · p. 4
D. Gradient Derivation
The gradient derivation turns continuous trajectory constraints into sampled constraints and propagates their derivatives through trajectory coefficients and timing. The resulting gradients depend on the constraint-specific derivatives with respect to flat-space trajectory values and time.
D. Gradient Derivation
- Continuous-time feasibility constraints are enforced by uniformly discretizing each polynomial piece into λ constraint points.The paper states that whole-trajectory feasibility is equivalent to each piece satisfying the constraints before applying this discretization.Source: D. Gradient Derivation · p. 4
- The derivation applies the chain rule at each constraint point to obtain constraint gradients with respect to trajectory coefficients and segment times.Time-related gradient terms are included in the propagation.Source: D. Gradient Derivation · pp. 4–5
- Constraint-specific gradients are obtained from derivatives with respect to flat-space trajectory values and absolute time, with irrelevant-variable derivatives set to zero.The paper first derives objective gradients with respect to coefficients and durations, then specifies constraint functions and their derivatives.Source: D. Gradient Derivation · pp. 4–5
IV. INSTANTANEOUS STATE CONSTRAINTS
The section formulates instantaneous state constraints for velocity and acceleration. These limits reflect practical operating bounds and tire-ground friction constraints.
IV. INSTANTANEOUS STATE CONSTRAINTS
- The section focuses on constraints that depend only on the vehicle’s instantaneous state.Source: §IV · p. 5
1) Longitude Velocity Limit:
- Longitudinal velocity is constrained to a reasonable range for traffic rules, vehicle performance, and environmental uncertainty.The constraint is defined at a trajectory constraint point, with gradients obtained by combining the constraint with earlier flat-output equations.Source: 1) Longitude Velocity Limit · p. 5
2) Acceleration Limit:
- Longitudinal and lateral accelerations are constrained to prevent skidding from tire-ground friction limits.The section defines both acceleration constraint functions at a constraint point and derives their gradients.Source: 2) Acceleration Limit · p. 5
3) Front Steer Angle Limit:
The section limits front steering through a curvature bound and models static safety using a corridor of convex free-space polygons. Static constraints keep the vehicle’s full shape within the corridor at each constraint point.
3) Front Steer Angle Limit:
- The front steer angle is bounded by limiting curvature κ = tan φ/L to [−κ_m, κ_m], where κ_m corresponds to the preset maximum steer angle.This constraint is used to ensure nonholonomic dynamic feasibility.Source: 3) Front Steer Angle Limit · p. 5
B. Static Obstacle Avoidance
- Static safety is enforced by confining the ego vehicle’s full shape to a corresponding convex polygon at each constraint point.The corridor is formed from convex free-space polygons generated around samples of a collision-free path; proper trajectory resolution can practically guarantee static no-collision.Source: B. Static Obstacle Avoidance · pp. 5–6, Figure 3 · p. 6
- The full vehicle shape is contained in a corridor polygon when every vehicle vertex lies inside it, equivalently on the inner side of each polygon hyperplane.The resulting spatial constraint function contains linear penalties for vehicle vertices against corridor hyperplanes.Source: B. Static Obstacle Avoidance · p. 6
V. DYNAMIC OBSTACLE AVOIDANCE
Dynamic safety is formulated through the minimum distance between the ego vehicle and obstacle polygons over the trajectory. The section introduces signed distance to address the lack of separating gradients when objects overlap, while noting that exact computation is unsuitable for embedding in the optimizer.
V. DYNAMIC OBSTACLE AVOIDANCE
- Dynamic safety requires the minimum distance between the ego vehicle and obstacle polygons to exceed a safety threshold at each trajectory moment.Source: §V · p. 6
1) Signed Distances for Rigid Objects:
- When two objects overlap, ordinary distance is zero and provides no gradient direction for separating them, so penetration depth is combined with distance to define signed distance.Source: 1) Signed Distances for Rigid Objects · p. 6
- Exact signed-distance computation requires minimum-optimization problems, making it unsuitable for direct embedding in trajectory optimization.The following subsection discusses an efficient Minkowski-difference-based approximation.Source: 1) Signed Distances for Rigid Objects · p. 6
2) Approximation Distances:
The paper approximates signed distance between convex sets using bounds based on the Minkowski difference and distances to hyperplanes. It uses this approximation in dynamic-obstacle constraints and smooths discrete extrema to obtain continuously differentiable expressions.
2) Approximation Distances:
- For convex sets, signed distance is bounded using maximum signed distances to hyperplanes; checking vertices converts the bound to analytically calculable point-to-hyperplane distances.The construction uses the Minkowski difference, reducing the set-distance problem to distance from the origin to a transformed set.Source: 2) Approximation Distances · pp. 6–7, Figure 4 · p. 7
B. Constraint for Dynamic Avoidance
- Dynamic avoidance applies constraints at trajectory points using the moving obstacles’ trajectories at the same timestamps, requiring ego-obstacle distance to exceed the safety margin.The constraint is defined for each dynamic obstacle, using a lower approximation of signed distance between convex objects.Source: B. Constraint for Dynamic Avoidance · p. 7
- Log-sum-exp smooths vector maximum and minimum operations, with positive α approximating the maximum and negative α selecting the minimum.Its gradient is the softmax or softmin, and the resulting dynamic constraint is continuously differentiable and analytically expressible in flat outputs.Source: B. Constraint for Dynamic Avoidance · p. 8
VI. REFORMULATION OF TRAJECTORY OPTIMIZATION
The section analyzes trajectory-planning constraints and targets them for elimination, reformulating the original problem as an unconstrained program for efficient solution.
VI. REFORMULATION OF TRAJECTORY OPTIMIZATION
- The reformulation analyzes the trajectory-planning constraints in Eq. (5c–5f) and (9).It applies targeted approaches to eliminate the constraints.Source: §VI · p. 8
- The original constrained optimization is reformulated as an unconstrained program.The section frames this reformulation as a step toward efficient solution.Source: §VI · p. 8
- Different constraints are addressed with targeted elimination approaches.The passage does not specify the individual approaches here.Source: §VI · p. 8
A. Feasibility Constraints
Feasibility constraints are relaxed with a discrete-time penalty that is designed to be differentiable and non-negative, while gradients are propagated analytically.
A. Feasibility Constraints
- The discrete-time summation penalty SΣ relaxes the feasibility constraints in Eq. (9).Its terms use constraint-specific weights and trapezoidal-rule quadrature coefficients.Source: A. Feasibility Constraints · pp. 8–9
- The first-order relaxation L1(·) makes penalty terms continuously differentiable and non-negative.The demarcation point is a0 = 10^-4.Source: A. Feasibility Constraints · p. 9
- The discrete penalty formulation satisfies continuous-time constraints Eq. (5g) within an acceptable tolerance.Gradients of violation penalties and the resulting objective are obtained by propagation from constraint gradients.Source: A. Feasibility Constraints · p. 9
B. Equality Constraints
The equality constraints are removed by reparameterizing the optimization with waypoints and gear-shift variables, while retaining optimality and enabling efficient optimization.
B. Equality Constraints
- The velocity direction reverses at a gear shift, and its magnitude is set to a small non-zero value to prevent optimization singularities.The implementation uses a magnitude of 0.05.Source: B. Equality Constraints · p. 9
- Waypoints become decision variables without sacrificing optimality because minimum-control-effort piecewise-polynomial coefficients are uniquely determined by waypoints, durations, and boundary states.The continuity degree is set to 2s−1 to satisfy the optimality condition.Source: B. Equality Constraints · p. 9
- The cost is reparameterized using the gear-shift angles, with gradients derived for optimization over those variables.The passage also notes that gradients from polynomial coefficients to time and waypoints can be computed with linear complexity.Source: B. Equality Constraints · p. 9
- All equation constraints in the original planning problem are eliminated as the basis for subsequent efficient optimization.The minimum-control-effort coefficients are determined from the reduced decision variables.Source: B. Equality Constraints · p. 9
C. Positiveness Condition
Strictly positive durations are represented through an unconstrained virtual-time variable, allowing the optimization to be fully unconstrained and solved with a quasi-Newton method.
C. Positiveness Condition
- An unconstrained virtual time τ is mapped to real durations Ti in R+ to remove the strict-positiveness condition.The real-time variables are replaced by τ, so duration positivity is satisfied naturally.Source: C. Positiveness Condition · pp. 9–10
- The transformed problem uses q, τ, pg, and θg as optimization variables.Its objective is the reparameterized cost K(q, T(τ), pg, θg).Source: C. Positiveness Condition · p. 10
- The transformed unconstrained optimization is solved with a quasi-Newton method after constraints are removed and gradients are derived analytically.Gradients with respect to τ are calculated from those with respect to T.Source: C. Positiveness Condition · p. 10
VII. EVALUATIONS
The evaluations report faster planning and better trajectory quality in simulation benchmarks, alongside dynamic-scene and physical-platform demonstrations. The conclusion identifies broader robot models and rugged terrain as future work.
A. Simulations
- Benchmarks report an order-of-magnitude efficiency advantage and better trajectory quality; simulations also show the lowest control effort and best human comfort.The time comparison is visualized in Fig. 8, and dynamic statistics are compared in Table I.Source: A. Simulations · p. 10, §VII · p. 10, Figure 8 · p. 11
- H-OBCA produces dynamics statistics close to the proposed planner’s, but is inefficient, cannot run in real time, and incurs greater computational burden as obstacles increase.The paper attributes its high-quality trajectories to accurate problem modeling.Source: A. Simulations · p. 11
- In the 210 m × 50 m dynamic scene, the 260 m ego trajectory took 0.18 s and showed no spatiotemporal intersections with eight other vehicles.The other vehicles’ trajectories and the environment were known to the planner.Source: A. Simulations · p. 11, Figure 7 · p. 11
B. Real-World Experiments
- On a physical Baojun E3002, the planner navigated a 42 m outdoor obstacle course and reversed into a user-heading parking space while respecting dynamic limits.The real-world experiment used an outdoor unstructured parking lot; Fig. 11 shows the execution trajectory.Source: B. Real-World Experiments · pp. 11–12, Figure 11 · p. 13
VIII. CONCLUSION
- Flat-space polynomials reduce decision variables while representing continuous trajectories with higher-order information; safe corridors and signed distances handle static and moving obstacles.The conclusion reports benchmark superiority in time efficiency and trajectory quality, and real-world validation on a physical platform.Source: §VIII · pp. 12–13
- Future work includes applying the method to multiple robots and other motion models, and extending planning to rugged terrain.Source: §VIII · p. 13