Reachability-Augmented Dual Dynamic Programming for Optimal Path Parameterization

Abstract

Optimal path parameterization (OPP) is a fundamental problem for planning trajectories along a prescribed geometric path under kinodynamic constraints and task-dependent objectives. Among existing OPP works, it remains challenging to simultaneously achieve feasibility guarantees, general-objective optimality certificates, and online-compatible computation 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 dynamic programming (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 second-order OPP (OPP2) and OPP3. Experiments show that RDDP achieves objective values comparable to convex-optimization baselines with speedups of approximately 15.3 and 4.9 times over the fastest of 4 tested solvers for OPP2 and OPP3, respectively. 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 framework.