LTO: Lazy Trajectory Optimization with Graph-Search Planning for High DOF Robots in Cluttered Environments

TL;DR

LTO combines local trajectory optimization with global graph-search planning to efficiently generate high-quality, long-horizon trajectories for high-DOF robots in cluttered environments, improving speed and reliability.

Contribution

This paper introduces Lazy Trajectory Optimization (LTO), a novel framework unifying local TO and global GSP, with a new cost function and proofs of complexity and optimality.

Findings

LTO outperforms existing algorithms in runtime.

LTO achieves higher planning reliability.

LTO effectively handles high-DOF robots in cluttered spaces.

Abstract

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 solves TO with the same constraints as the original long-horizon TO with improved time complexity. We also propose a TO-aware cost function that can balance both solution cost and planning time. 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 optimality of LTO. 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.