Adaptive Trajectory Refinement for Optimization-based Local Planning in Narrow Passages

TL;DR

This work addresses the challenge of kinodynamic local planning in narrow passages by augmenting a Timed Elastic Band (TEB) trajectory with adaptive refinement. The approach introduces segment-wise continuous collision detection (CCD) to conservatively identify collision-prone segments and a PD-based pose correction with line search to relocate poses away from obstacles, followed by local kinematic feasibility adjustments. The key contributions include a sparsified yet obstacle-adjacent waypoint distribution, a fast CCD-driven refinement loop, and a geometry-aware pose relocation strategy that maintains kinodynamic feasibility. Empirical results show up to 1.69x higher success rates and up to 3.79x faster planning in simulation, with robust real-world performance in narrow passages, indicating practical benefits for real-time navigation in cluttered environments.

Abstract

Trajectory planning for mobile robots in cluttered environments remains a major challenge due to narrow passages, where conventional methods often fail or generate suboptimal paths. To address this issue, we propose the adaptive trajectory refinement algorithm, which consists of two main stages. First, to ensure safety at the path-segment level, a segment-wise conservative collision test is applied, where risk-prone trajectory path segments are recursively subdivided until collision risks are eliminated. Second, to guarantee pose-level safety, pose correction based on penetration direction and line search is applied, ensuring that each pose in the trajectory is collision-free and maximally clear from obstacles. Simulation results demonstrate that the proposed method achieves up to 1.69x higher success rates and up to 3.79x faster planning times than state-of-the-art approaches. Furthermore, real-world experiments confirm that the robot can safely pass through narrow passages while maintaining rapid planning performance.

Figures (6)

• Figure 1: Limitations of TEB optimization (a) $\mathbf{o}_3$ is the closest to $\mathbf{x}_3$. The trajectory can penetrate into obstacles since (b) $\lVert \mathbf{x}_3-\mathbf{o}_3\rVert = \lVert \mathbf{x}_3' - \mathbf{o}_3\rVert$ and (c) $\mathbf{x}_3'$ yields a shorter trajectory than $\mathbf{x}_3$. (d) Waypoints are sparsely determined near obstacles, leading to collisions. (e) The original temporal resolution in the trajectory. (f) Reducing the resolution can cause collisions.
• Figure 2: Algorithm Overview. (a) An initial plan from TEB hyper-graph optimization. (b–d) Iterative collision detection, pose correction, and orientation update make all poses collision-free. (e-f) Segment-wise CCD subdivides risky segments, with new poses refined through (b-d). (g) A collision-free trajectory is obtained. This trajectory is then fed back to the hyper-graph optimization, forming an iterative planning pipeline
• Figure 3: Separation directions $\mathbf{v}$ determined when (a) the robot's mid-position is located at $\mathbf{x}$ inside an obstacle (gray grids), (b) on the obstacle boundary (red grids), (c) outside the obstacle but still close to the obstacle.
• Figure 4: Gazebo simulation environment from the BARN dataset annotated with benchmark number. The Jackal robot navigates through 300 different environments with varying obstacle densities, represented by red cylinders.
• Figure 5: Experimental results across simulation environment. Experimental results across 300 BARN environments, showing traversal time, planning time, and maximum planning time for TEB(pink), egoTEB(green), and the proposed method(orange). Each point corresponds to an individual record in a given benchmark world, while the red markers at the top indicate the failure case.