TL;DR
This paper introduces a high-performance algorithm for planning smooth, collision-free trajectories through complex environments represented by thousands of axis-aligned "safe boxes." By combining offline graph preprocessing with a two-stage online optimization process, the method achieves real-time performance on large-scale problems that were previously computationally prohibitive.
What Problem Does This Paper Address?
Path planning for autonomous systems like drones or mobile robots requires finding paths that are not only collision-free but also smooth (e.g., minimizing jerk or snap for stable flight). A common approach is to decompose the free space into a collection of safe convex regions.
However, existing methods face a scalability wall. Techniques like Graphs of Convex Sets (GCS) provide strong optimality guarantees but struggle when the number of safe regions exceeds a few hundred. Alternatively, Mixed-Integer Programming (MIP) approaches are notoriously slow, often taking minutes to solve even medium-sized problems. The challenge addressed here is: how can we plan smooth, guaranteed-safe trajectories through environments containing tens of thousands of safe boxes in just a few seconds?
Methods
The authors propose a three-phase approach, leveraging the geometric simplicity of axis-aligned boxes to accelerate computation.
1. Offline Preprocessing
The algorithm first builds a "line graph" representing the connectivity of the safe boxes. Vertices in this graph correspond to the intersections of boxes, and edges connect intersections within the same box. A key innovation is optimizing "representative points" within these intersections to minimize total edge length, creating a high-quality skeleton for future paths.
2. Online Polygonal Phase
When a start and end point are given, the algorithm quickly finds a shortest path in the precomputed graph. It then enters an iterative shortening loop, alternating between:
- Optimizing node positions via a Second-Order Cone Program (SOCP).
- Updating the box sequence by checking if inserting new boxes can further reduce path length.
3. Online Smooth Phase
The polygonal path is transformed into a smooth trajectory parameterized by piecewise Bézier curves. Because Bézier curves lie within the convex hull of their control points, the path is guaranteed to be safe at all times if the control points stay within the safe boxes.
To solve the resulting nonconvex optimal-control problem, the authors use convex alternations:
- Projection Problem: Fixes traversal times and optimizes path shape (a convex Quadratic Program).
- Tangent Problem: Uses a trust-region SOCP to improve traversal times based on a local convex approximation.
Datasets
The algorithm was evaluated on several benchmarks:
- Scaling Study: Synthetic 2D environments with random axis-aligned boxes, scaling from 25 to 25,600 boxes.
- Large-Scale Quadrotor Flight: A simulated 3D "village" (buildings, trees, bushes) decomposed into 10,150 safe boxes.
- MIQP Comparison: A small-scale 2D example used to benchmark against state-of-the-art commercial solvers like CPLEX and Gurobi.
Key Insights and Ideas
- Box Geometry as a Speedup: Restricting safe sets to axis-aligned boxes makes intersection and "stabbing" (point-in-box) queries extremely fast, which is the secret to their massive scalability.
- Completeness without Global Optimality: The algorithm is "complete"—it will always find a path if one exists—but it sacrifices global optimality guarantees to achieve its speed.
- Bézier Curve Safety: By constraining Bézier control points rather than just sampling points along the path, the algorithm provides continuous-time safety guarantees.
- Linear Scaling: The online computation time grows only linearly with the number of boxes traversed by the path, not the total number of boxes in the environment.
Key Results
- Massive Scalability: The planner handled 25,600 boxes in approximately 3.5 seconds of online time. For comparison, GCS typically handles fewer than 100 boxes in the same timeframe.
- Real-time Quadrotor Planning: In the 10,000-box "village" example, a smooth, snap-minimized trajectory was generated in just 7.7 seconds.
- Efficiency vs. MIP: For a small 9-box problem, the proposed method was 26 times faster than the best commercial MIP solver (CPLEX) while achieving a lower path cost because it could finely optimize traversal times.
- Near-Optimality: In practical tests, the heuristic cost was often within 0.01% of the global minimum.
Limitations and Future Work
- Static Environments: The heavy reliance on offline preprocessing means the algorithm is best suited for static environments. Dynamic obstacles would require re-running parts of the pipeline.
- Heuristic Nature: While fast and effective, the lack of formal optimality bounds might be a drawback for certain safety-critical applications requiring strict performance guarantees.
- Axis-Aligned Constraint: The efficiency gains are tied to the box representation. While complex shapes can be decomposed into boxes, this introduces approximation error (conservatism).
- Future Extensions: The authors suggest extending the framework to handle moving obstacles, multiple waypoints, and periodic trajectories.
The implementation is available as an open-source Python package: fastpathplanning.
Summary updated