Reachability-Augmented Dual Dynamic Programming for Optimal Path Parameterization

TL;DR

This paper introduces RDDP, a novel dynamic programming framework for optimal path parameterization that guarantees feasibility, certifies optimality, and improves computational efficiency for complex kinodynamic planning tasks.

Contribution

RDDP replaces classical assumptions with reachability sets, enabling unified, certifiable, and efficient optimal path parameterization for both convex and non-convex problems.

Findings

RDDP achieves comparable objective values to convex optimization methods.

RDDP reduces computation time by up to 28.6 times for OPP2.

RDDP converges faster than grid-based dynamic programming.

Abstract

Optimal path parameterization (OPP) is a fundamental problem for planning trajectories along a prescribed geometric path under kinodynamic constraints and task-dependent objectives. While TOPP minimizes traversal time, its saturating states and controls may induce vibration and tracking errors, which can be mitigated by introducing smoothness objectives. However, a key capability gap remains in OPP: feasibility guarantees, general-objective optimality certificates, and computational efficiency are difficult to achieve simultaneously in a unified framework, especially for third-order OPP (OPP3) with non-convex constraints. This paper proposes reachability-augmented dual dynamic programming (RDDP), a state-grid-free and objective-aware DP framework for OPP. The key idea is to replace the relatively complete recourse assumption used in classical dual DP (DDP) with OPP-specific backward reachable sets, and then generate both value-function cuts and trial trajectories only inside these reachable sets. For convex and non-convex OPP, we prove global optimality and Karush-Kuhn-Tucker convergence of RDDP under OPP-specific conditions, respectively. Efficient instantiations are developed for OPP2 and OPP3. Experiments show that RDDP achieves objective values comparable to convex-optimization baselines while reducing computation time by 28.6 times for OPP2 and 5.8 times for OPP3. RDDP also achieves faster convergence than grid-based DP. Compared with reachability-analysis methods, RDDP retains the reachability mechanism while replacing local maximum-control propagation with value-function-guided control selection, thereby enabling objectives beyond traversal time. In summary, RDDP addresses a key capability gap in OPP by unifying certifiable general-objective optimization, reachability-based feasibility preservation, and online-compatible low-dimensional DP computation in a single OPP framework.