Topological Motion Planning Diffusion: Generative Tangle-Free Path Planning for Tethered Robots in Obstacle-Rich Environments

Abstract

In extreme environments such as underwater exploration and post-disaster rescue, tethered robots require continuous navigation while avoiding cable entanglement. Traditional planners struggle in these lifelong planning scenarios due to topological unawareness, while topology-augmented graph-search methods face computational bottlenecks in obstacle-rich environments where the number of candidate topological classes increases. To address these challenges, we propose Topological Motion Planning Diffusion (TMPD), a novel generative planning framework that integrates lifelong topological memory. Instead of relying on sequential graph search, TMPD leverages a diffusion model to propose a multimodal front-end of kinematically feasible trajectory candidates across various homotopy classes. A tether-aware topological back-end then filters and optimizes these candidates by computing generalized winding numbers to evaluate their topological energy against the accumulated tether configuration. Benchmarking in obstacle-rich simulated environments demonstrates that TMPD achieves a collision-free reach of 100% and a tangle-free rate of 97.0%, outperforming traditional topological search and purely kinematic diffusion baselines in both geometric smoothness and computational efficiency. Simulation with realistic cable dynamics further validates the practicality of the proposed approach.

Figures (6)

• Figure 1: Paths $\tau_1$ and $\tau_2$ (blue) belong to the same homotopy class as they can be continuously deformed into each other within the obstacle-free space. Path $\tau_3$ (orange) represents a distinct homotopy class, circumventing $O_i$ from the opposite side. The generalized winding number evaluates the accumulated angle $\int d\theta$ along the path relative to the obstacle center $o_i$.
• Figure 2: At the $k$-th step, the robot at $g_{k-1}$ is constrained by the history trajectory $\tau_{hist}^{(k)}$ (blue solid line) anchored at $g_0$. A kinematically feasible but topologically naive segment $\tau^{(k)}$ (red dashed line) heading directly to $g_k$ causes the global trajectory $\tau_{global}^{(k)}$ to entangle around obstacle $O_i$, violating the threshold $|W| < W_{th}$. $\tau^{(k)*}$ (green dashed line) is a valid tangle-free trajectory.
• Figure 3: The robot currently resides at $g_{k-1}$, with the dark blue line representing the executed history trajectory $\tau_{hist}^{(k)}$ (i.e., the current physical cable configuration). The diffusion model generates a candidate pool $\mathcal{T}_{cand}$ (red lines) connecting $g_{k-1}$ to the target $g_k$, exploring diverse topological branches ($\mathcal{H}_1 \dots \mathcal{H}_5$). Tthe topological back-end then concatenates a specific candidate—in this case, a trajectory from the $\mathcal{H}_4$ branch—with the history. The cyan dashed line illustrates its resulting global taut configuration $\text{Hom}(\tau_{global}^{(k)})$.
• Figure 4: While MPD (blue) often produces irregular trajectories that unnecessarily skirt obstacle boundaries, our TMPD (dark red) utilizes topological energy and kinematic penalties to recover optimal, smooth, and safe paths.
• Figure 5: From left to right: Topo-A*, Topo-RRT, MPD, and our proposed TMPD. The grey circles represent randomly added obstacles, while the red squares denote fixed obstacles used during training. The blue lines indicate the execution history.