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

TL;DR

This paper introduces an adaptive trajectory refinement method that enhances local planning in narrow passages by recursively subdividing risky segments and correcting poses, resulting in higher success rates and faster planning.

Contribution

The proposed algorithm combines segment-wise collision testing and pose correction to improve planning success and efficiency in narrow, cluttered environments.

Findings

Achieves up to 1.69x higher success rates

Up to 3.79x faster planning times

Successfully passes through narrow passages in real-world tests

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.