Collision Avoidance using Iterative Dynamic and Nonlinear Programming with Adaptive Grid Refinements

TL;DR

The paper tackles collision-avoidant trajectory planning for high-dimensional dynamical systems with obstacles by marrying dynamic programming on a low-dimensional representation with nonlinear programming in the full space, augmented by adaptive state-space grid refinement. The approach uses a penalty term $\rho \mathcal{P}(w)$ and a terminal/feasibility term $\mathcal{M}(w(\tau_f))$ within a DP framework linked to the high-dimensional space via the mapping $\Omega$. A key contribution is replacing penalty-based adaptation with adaptive grid refinement, leveraging cube-based grids from $\text{richter2023adaptive}$ to capture narrow passages efficiently and improve convergence. Numerical results on a space-manipulator demonstrate higher success rates and robustness compared to fixed-grid IDNP and a full discretization, with substantial reductions in memory and compute time. Overall, the method offers a scalable, robust framework for complex obstacle-avoidant planning where global feasibility and high-dimensional dynamics must be reconciled.

Abstract

Nonlinear optimal control problems for trajectory planning with obstacle avoidance present several challenges. While general-purpose optimizers and dynamic programming methods struggle when adopted separately, their combination enabled by a penalty approach is capable of handling highly nonlinear systems while overcoming the curse of dimensionality. Nevertheless, using dynamic programming with a fixed state space discretization limits the set of reachable solutions, hindering convergence or requiring enormous memory resources for uniformly spaced grids. In this work we solve this issue by incorporating an adaptive refinement of the state space grid, splitting cells where needed to better capture the problem structure while requiring less discretization points overall. Numerical results on a space manipulator demonstrate the improved robustness and efficiency of the combined method with respect to the single components.

Figures (4)

• Figure 1: Schematic overview of the proposed iterative scheme for motion planning in constrained environments: low-dimensional collision-free waypoints from dynamic programming (DP) are mapped back to the original dimension and tracked by a trajectory planned with nonlinear programming (NLP). Possible collisions are integrated into the DP via a penalty term and the DP state space grid is adapted, until convergence to a valid solution.
• Figure 2: Splitting process of 2D grid $\mathbb{G}_w^j$ (black) with additional points after update according to richter2023adaptive (blue), due to the collection of waypoints in collision $\{\tau_k,w_k\}_{k = 0}^{K}$ (green) and those belonging to this grid $\mathbb{K}_w^j$ (red).
• Figure 3: Illustration of the satellite body with robotic arm. The coordinate axes of the satellites inertial frame $I$ are highlighted.
• Figure 4: Illustration of state space grids and waypoints at DP iteration 6 resulting from iterations 1, 2, and 3 of Algorithm \ref{['alg:IterativeScheme']} using initial condition $\tilde{q}_0 = \{-0.2,-0.14,-0.68,-0.49,0.34,0.43,0.13,-0.02,0.1\}$. The goal region (light blue) as well as the obstacles (gray) are highlighted.