STITCHER: Real-Time Trajectory Planning with Motion Primitive Search

Autonomous high-speed navigation through large, complex environments requires real-time generation of agile trajectories that are dynamically feasible, collision-free, and satisfy constraints. Most modern trajectory planning techniques rely on numerical optimization because high-quality, expressive trajectories that satisfy constraints can be systematically computed. However, strict requirements on computation time and the risk of numerical instability can limit the use of optimization-based planners in safety-critical situations. This work presents an optimization-free planning framework called STITCHER that leverages graph search to generate long-range trajectories by stitching short trajectory segments together in real time. STITCHER is shown to outperform modern optimization-based planners through its innovative planning architecture and several algorithmic developments that make real-time planning possible. Simulation results show safe trajectories through complex environments can be generated in milliseconds that cover tens of meters.

💡 Research Summary

The paper “STITCHER: Real-Time Trajectory Planning with Motion Primitive Search” presents a novel, optimization-free framework for generating long-range, dynamically feasible trajectories in real-time, addressing critical challenges in autonomous high-speed navigation through complex environments.

The core problem is that while numerical optimization-based planners can produce high-quality, constraint-satisfying trajectories, they suffer from unpredictable computation times and potential numerical instability, making them less suitable for safety-critical applications. Conversely, methods that continuously replan using short trajectory segments (motion primitives) are computationally efficient but can exhibit myopic and suboptimal behavior over long distances.

STITCHER bridges this gap by employing graph search to stitch together motion primitives. Its key innovation is a three-stage planning architecture designed for real-time performance. In Stage 1, a sparse geometric path (sequence of waypoints) through the environment is generated using a standard graph search algorithm like A* on a voxel grid. This path confines the subsequent search space. Stage 2 is crucial for efficiency. It constructs a “velocity graph” where nodes represent combinations of waypoints and sampled velocities. Using a control-constrained double integrator model (which allows for very fast, closed-form minimum-time calculations), dynamic programming is applied backward through this graph to compute the optimal “cost-to-go” (minimum time to goal) for every node. Critically, the authors prove this cost-to-go serves as an admissible heuristic for the next stage.

Stage 3 performs the final motion primitive search. Guided by the heuristic from Stage 2, an A* search is conducted over a graph where edges are motion primitives generated from a higher-order dynamical model (e.g., leveraging differential flatness for quadrotors). This stage considers the full state. Crucially, during this search, candidate motion primitives are checked for collisions and complex state/actuator constraints (like thrust magnitude and tilt limits) using an efficient sampling-based method. The paper also introduces an optimization for collision checking that reuses known free-space information from previously evaluated trajectories, further reducing computation.

Theoretical analysis proves that the constructed graphs are finite and that the Stage 2 heuristic is admissible, guaranteeing that the A* search in Stage 3 will find an optimal trajectory with respect to the graph discretization and has predictable time/memory complexity bounds—a significant advantage for safety-critical systems.

The framework was extensively evaluated in simulation and compared against two state-of-the-art real-time optimization-based planners. Results demonstrated that STITCHER consistently generated trajectories significantly faster (in milliseconds) while achieving comparable trajectory execution times, successfully planning safe paths covering tens of meters in complex environments. This work provides a compelling alternative to optimization-based planning, offering a blend of real-time performance, predictability, and the ability to generate long-horizon, non-myopic trajectories.