High-Dof Robots In Dynamic Environments Using Incremental Trajectory Optimization

We present a novel optimization-based motion planning algorithm for high degree-of-freedom (DOF) robots in dynamic environments. Our approach decomposes the high-dimensional motion planning problem into a sequence of low-dimensional sub-problems. We compute collision-free and smooth paths using optimization-based planning and trajectory perturbation for each sub-problem. The overall algorithm does not require a priori knowledge about global motion or trajectories of dynamic obstacles. Rather, we compute a conservative local bound on the position or trajectory of each obstacle over a short time and use the bound to incrementally compute a collision-free trajectory for the robot. The high-DOF robot is treated as a tightly coupled system, and we incrementally use constrained coordination to plan its motion. We highlight the performance of our planner in simulated environments on robots with tens of DOFs.