Abstract
As a core part of autonomous driving systems, motion planning has received extensive attention from academia and industry. However, there is no efficient trajectory planning solution capable of spatial-temporal joint optimization due to nonholonomic dynamics, particularly in the presence of unstructured environments and dynamic obstacles. To bridge the gap, we propose a versatile and real-time trajectory optimization method that can generate a high-quality feasible trajectory using a full vehicle model under arbitrary constraints. By leveraging the differential flatness property of car-like robots, we use flat outputs to analytically formulate all feasibility constraints to simplify the trajectory planning problem. Moreover, obstacle avoidance is achieved with full dimensional polygons to generate less conservative trajectories with safety guarantees, especially in tightly constrained spaces. We present comprehensive benchmarks with cutting-edge methods, demonstrating the significance of the proposed method in terms of efficiency and trajectory quality. Real-world experiments verify the practicality of our algorithm.
Introduction
Motion planning for autonomous vehicles is a complex balancing act involving nonholonomic dynamics, full-dimensional obstacle avoidance, and computational efficiency. Traditional systems often simplify vehicle shapes into circles to ease calculations, but this "conservative" approach prevents vehicles from entering narrow gaps they could physically fit through. Furthermore, many planners separate path and timing optimization, leading to jerky, sub-optimal trajectories in dynamic environments.
This research introduces a high-speed trajectory planner that solves these issues simultaneously. By enabling real-time, spatial-temporal joint optimization, the system allows vehicles to navigate tight, unstructured environments with human-like smoothness.
Methodology
The core innovation of this framework is the application of differential flatness. Car-like robots possess a property where their entire state—including position, heading, velocity, and steering angle—can be represented by a "flat output" (the position of the rear axle) and its derivatives. This allows the optimization of simple polynomial curves instead of complex differential equations.
Key components of the method include:
- Safe Driving Corridors: For stationary obstacles, the system generates overlapping convex polygons that define the vehicle's navigable space.
- Signed Distance Approximations: To handle dynamic objects, the authors use a Minkowski difference-based approach to calculate distances between the vehicle's rectangular body and other objects. This ensures the car utilizes "dead space" in corners, allowing it to slalom through narrow gaps.
- Unconstrained Reformulation: By turning constraints into penalty functions, the system can use high-efficiency quasi-Newton solvers, making the entire process analytically differentiable and extremely fast.
Evaluation and Results
The system was rigorously tested in the CARLA Simulator and on a modified SAIC-GM-Wuling Baojun E300 electric vehicle equipped with a TI TDA4 embedded chip.
Key Findings
- Speed: In scenarios with over 20 obstacles, the proposed planner was 10x to 100x faster than high-quality benchmarks like H-OBCA, typically generating trajectories in under 0.05 seconds.
- Quality: Trajectories exhibited the lowest mean acceleration and jerk across test cases, indicating a smoother ride.
- Real-World Practicality: The planner successfully navigated a dense 40m x 20m parking lot on an embedded platform, performing complex maneuvers like reversing into spots with minimal computation time.
Conclusion and Future Work
By modeling the vehicle as a convex polygon and leveraging differential flatness, this planner achieves a significant leap in both efficiency and trajectory quality. While the system currently depends on a "front-end" initial guess (such as Hybrid A*), future research will focus on extending the framework to multi-robot coordination and 3D environments, such as rugged terrain or aerial navigation.
Summary updated