Dynamic and Nonlinear Programming for Trajectory Planning

Abstract

Direct optimal control techniques, relying on numerical methods for constrained optimization, are typically used in trajectory planning tasks in high-dimensional spaces. However, general-purpose solvers often fail to find a feasible solution when facing cluttered environments. Sampling-or graph-based methods, instead, can explore complex configuration spaces but struggle with dynamic constraints. Here, we propose to combine dynamic programming (DP) and derivative-based methods to reliably solve trajectory planning problems. Specifically, we exploit DP to generate a sequence of waypoints in a lowdimensional space, which are then encoded as pointwise path constraints for a high-dimensional trajectory, whose constraint violations are then represented as a penalty within the Bellman equation to recompute the waypoints. This iterative approach, alternating path and trajectory optimization, avoids both the curse of dimensionality for DP and problematic nonconvexities (such as obstacles) for motion planning. We demonstrate our strategy using numerical experiments on a six-degree-of-freedom robotic manipulator moving in a confined space.