You can find the paper on arxiv!

Although Trajectory Optimization (TO) is one of the most powerful motion planning tools, it suffers from expensive computational complexity as a time horizon increases in cluttered environments. It can also fail to converge to a globally optimal solution. In this paper, we present Lazy Trajectory Optimization (LTO) that unifies local short-horizon TO and global Graph-Search Planning (GSP) to generate a long-horizon global optimal trajectory. LTO enables a robot to solve TO with the same constraints as original long-horizon TO with improved time complexity. We also propose a TO-aware cost function that enables LTO to balance both solution cost and planning time. Furthermore, since LTO solves many nearly identical TO in a roadmap, it can provide an informed warm-start for TO to accelerate the planning process. We also present proofs of the computational complexity and suboptimality accounting for TO and GSP. Finally, we demonstrate LTO’s performance on motion planning problems for a 2 DOF free-flying robot and a 21 DOF legged robot, showing that LTO outperforms existing algorithms in terms of its runtime and reliability.