Dynamic Programming in Trajectory Search

• Dynamic Programming in Trajectory Search is an algorithmic framework that applies Bellman’s principle to compute optimal control sequences for complex dynamical systems.
• Differential Dynamic Programming leverages quadratic approximations and backward-forward recursions to optimize trajectories in high-dimensional, nonlinear systems.
• Advanced DP variants incorporate constraints, uncertainty, and adaptive grid strategies to enhance robustness and enable real-time control in challenging environments.

Dynamic programming (DP) in trajectory search refers to a family of algorithmic frameworks that exploit Bellman’s principle of optimality to systematically compute cost-optimal input (and, possibly, feedback) sequences for dynamical systems. In this context, “trajectory” denotes a path in the state (and possibly output) space generated by a sequence of admissible control actions under system dynamics. The dynamic programming principle underpins exact value-iteration methods for discretized state spaces as well as advanced methods such as Differential Dynamic Programming (DDP), which leverage local quadratic approximations to enable tractable trajectory optimization for high-dimensional, nonlinear systems. State-of-the-art variants further integrate stochasticity, constraints, belief space representations, hybrid continuous-discrete controls, and adaptive grid refinements, yielding robust and scalable algorithmic pipelines across a broad range of trajectory optimization scenarios.

1. Bellman Recursion and Discrete-Time Trajectory Search

At the core of dynamic programming is Bellman’s equation, which formulates trajectory optimization as the recursive minimization of a cost-to-go, or value function. In discrete time, given state $x_k \in \mathbb{R}^n$, control $u_k \in \mathbb{R}^m$, and horizon $N$, the optimal value function is

$$V_k(x_k) = \min_{u_k} \left[ \ell(x_k, u_k) + V_{k+1}(f(x_k, u_k)) \right]$$

with terminal cost $V_N(x) = \phi(x)$ and system dynamics $x_{k+1} = f(x_k, u_k)$. This recursive structure provides the foundation for all DP-based trajectory search algorithms, enabling global optimization (up to discretization error) for finite-horizon deterministic or stochastic problems (Stachowicz et al., 2021, Gandhi, 2015, Abbasov et al., 13 Mar 2025).

The dynamic programming recursion generalizes to handle hybrid controls (discrete + continuous), stochastic disturbances, belief-space (POMDP) representations, and constrained or partially observed systems by extending or augmenting the stage cost, state, and action spaces accordingly (Pajarinen et al., 2017, Fujiwara et al., 8 May 2026, Richter et al., 2023).

2. Differential Dynamic Programming: Second-Order Methods

The combinatorial explosion of state-space or control-space size precludes direct value iteration for high-dimensional systems. Differential Dynamic Programming (DDP), and its numerous modern variants, alleviate this by constructing local quadratic approximations to the Bellman backup (the so-called Q-function), yielding backward/forward passes analogous to Riccati-type recursions:

• Backward pass: Around a nominal trajectory $(\bar{x}_k, \bar{u}_k)$, linearize $f$ and quadraticize costs, then compute the second-order Q-function expansion. The local control law is

$$\delta u_k^* = -Q_{uu}^{-1} Q_u - Q_{uu}^{-1} Q_{ux} \delta x_k = k_k + K_k \delta x_k$$

and value function derivatives $V_{k,x}, V_{k,xx}$ propagate recursively (Stachowicz et al., 2021, Gandhi, 2015, Fan et al., 2017).

• Forward pass: Update the trajectory via

$u_k \in \mathbb{R}^m$0

typically with a line search on $u_k \in \mathbb{R}^m$1 for cost decrease.

DDP is exact for linear-quadratic problems in one backward-forward iteration (the discrete Riccati solution), and converges locally quadratically for smooth nonlinear systems (Stachowicz et al., 2021). For time-delayed or partially observed systems, the state and perturbations are suitably augmented (Fan et al., 2017, Fujiwara et al., 8 May 2026).

3. Extensions: Constraints, Uncertainty, and Hybrid Control

Practical trajectory optimization must accommodate constraints and uncertainty. Contemporary DP-based methods address this via:

• Augmented Lagrangian DDP: The primal-dual approach handles complex equality and inequality constraints by embedding merit or barrier functions into the stage and terminal costs, enabling Newton-like convergence and global convergence guarantees. Slack variables and penalty parameters are adaptively updated, and the backward pass incorporates Lagrange multiplier and constraint terms in the Q-function (Jallet et al., 2022, Singh et al., 2021, Alcan et al., 2023).
• Robust DDP: By formulating the inner minimax in the Bellman equation, local quadratic relaxations via multipliers and the S-procedure yield LMIs that accommodate worst-case disturbances and ensure upper bounding of the true cost-to-go (Gramlich et al., 2022).
• Hybrid control/action DDP: Discrete (hybrid) actions are embedded by “soft” convexification—the introduction of mixture weights $u_k \in \mathbb{R}^m$2 over action modes. The backward pass includes continuous and discrete components, with simplex constraints handled via QP projection. Shaping penalties enforce one-hot solutions as iterations progress (Pajarinen et al., 2017). Constraints on control, state, or mixture weights are managed via small per-stage QPs.
• Trajectory optimization on manifolds: For matrix Lie group states, DDP is implemented on the Lie algebra via local linearization and retraction, enabling efficient, constraint-handling trajectory optimization for pose and orientation (Alcan et al., 2023).

4. Adaptive and High-Dimensional DP for Trajectory Planning

For trajectory search in very high-dimensional spaces, pure dynamic programming is not tractable. The integration of DP with direct nonlinear programming (NLP)—the “iterative DP-NLP” paradigm—enables viable solutions by exploiting state abstraction and adaptive discretization:

• Low-dimensional DP / high-dimensional NLP: DP generates global low-dimensional waypoints (e.g., end-effector TCP positions). These are then “lifted” through inverse kinematics and refined via high-dimensional NLP under dynamics and collision constraints (Richter et al., 2023).
• Penalty-based obstacle avoidance and adaptive grid refinement: State and control grids adaptively subdivide only where constraints (e.g., collision, infeasibility) occur, concentrating computational burden on difficult regions. This ensures memory savings and improved convergence in complex environments (Richter et al., 2023, Abbasov et al., 13 Mar 2025).

Table: DP Grid Complexity Comparison

Scheme	#States	Memory Scaling	When Advantageous
Uniform grid (n-w)	$u_k \in \mathbb{R}^m$3	$u_k \in \mathbb{R}^m$4	Small $u_k \in \mathbb{R}^m$5, simple geometry
Adaptive grid	$u_k \in \mathbb{R}^m$6	$u_k \in \mathbb{R}^m$7	Narrow passage/obstacle rich

Here, $u_k \in \mathbb{R}^m$8 is the abstraction dimension, $u_k \in \mathbb{R}^m$9 is grid points/axis, $N$0 is #refined cells. Adaptive refinement drastically reduces computation where only selected regions are challenging (Richter et al., 2023).

5. Stochastic and Partial Observability Extensions

Trajectory search under stochasticity and limited observations motivates belief-space DP using policy and covariance propagation:

• Belief-space DDP (PO-SDDP): The state is represented by (mean, covariance) pairs, and the value function is defined over this belief. Prediction (via dynamics linearization) and filtering (EKF or UKF) update the belief, and the cost includes both nominal effort and covariance penalties. The backward pass generalizes the Riccati recursion to the belief space; the forward pass “rolls out” new controls and corresponding nominal/covariance trajectories. Coupling between trajectory design and estimation is explicit—no separation principle holds (Fujiwara et al., 8 May 2026, Pajarinen et al., 2017).
• Such approaches yield navigation- and estimation-aware trajectories, for example, exploiting more informative paths to reduce uncertainty at the cost of increased path length or fuel, as demonstrated in spacecraft transfer problems (Fujiwara et al., 8 May 2026).

6. Computational Complexity and Real-Time Considerations

Dynamic programming in trajectory search is subject to stringent computational and memory constraints. The per-iteration complexity of DDP and its modern constrained variants scales as $N$1, with $N$2 the horizon length, $N$3 state dimension, $N$4 control dimension (Gandhi, 2015, Jallet et al., 2022, Alcan et al., 2023, Stachowicz et al., 2021). Adaptive grid DP scales with the number of grid points, with memory and compute concentrated in critical regions. Real-time implementations leverage these algorithmic efficiencies to achieve millisecond-level recomputation, supporting applications including receding-horizon model predictive control and robust feedback law synthesis (Stachowicz et al., 2021, Richter et al., 2023).

Empirical benchmarks confirm DDP and its constrained, robust, and belief-space extensions achieve faster convergence and improved robustness versus purely open-loop or direct transcription methods, particularly under short horizons and locally smooth system dynamics. One-shot Riccati-based solutions are exact for linear-quadratic problems; Newton-type schemes and adaptive penalty update strategies guarantee convergence under broad conditions for the constrained and stochastic settings (Stachowicz et al., 2021, Jallet et al., 2022, Gramlich et al., 2022, Fujiwara et al., 8 May 2026).

7. Practical Implementation, Limitations, and Best Practices

Modern DP-based trajectory search algorithms integrate forward-backward passes, constraints/uncertainty handling, and adaptive discretizations into robust pipelines for real-world robotic and aerospace systems. Key considerations include:

• Initialization quality for DDP affects local minima avoidance in highly nonlinear systems (Gandhi, 2015).
• Adaptive regularization and penalty updating maintain numerical stability and convergence in constrained and robust settings (Jallet et al., 2022, Gramlich et al., 2022).
• Feedback gain extraction and control update law synthesis yield closed-loop robustness to model error and disturbance (Alcan et al., 2023, Singh et al., 2021).
• Curse of dimensionality remains a limiting factor for full-state DP except in low-dimensional settings or under aggressive abstraction/adaptive grid pruning (Richter et al., 2023, Abbasov et al., 13 Mar 2025).

Algorithmic advances continue to broaden the tractability, robustness, and application reach of dynamic programming in trajectory search, enabling efficient, high-confidence, and certifiably optimal (or locally optimal) solutions across deterministic, uncertain, hybrid, and partially observed systems.

References

• (Stachowicz et al., 2021, Pajarinen et al., 2017, Richter et al., 2023, Gramlich et al., 2022, Singh et al., 2021, Cao et al., 2021, Fujiwara et al., 8 May 2026, Jallet et al., 2022, Gandhi, 2015, Alcan et al., 2023, Fan et al., 2017, Abbasov et al., 13 Mar 2025)