VGTO: Visibility-Guided Topological Trajectory Optimization in Polyhedral Environments

Abstract

Planning safe and efficient flight trajectories for autonomous aerial robots in complex and unknown environments remains a challenging problem. Achieving high-quality local replanning is particularly difficult due to the inherent trade-off between computational efficiency and trajectory optimality, as well as the susceptibility of non-convex gradient-based methods to local minima. In this paper, we present VGTO, a visibility-guided topological trajectory optimization framework for real-time aerial navigation. By leveraging a lightweight polyhedral map, the proposed method eliminates the reliance on computationally expensive map representations, enabling efficient online replanning while preserving trajectory quality. Specifically, a visibility-guided topological path searching method is developed to directly generate diverse initialization paths without constructing an explicit topological graph. In addition, a lightweight obstacle distance and gradient evaluation method is introduced to enable efficient collision avoidance directly on the polyhedral map. Extensive simulations and real-world experiments demonstrate that the proposed method achieves a better balance between computational efficiency and trajectory quality, consistently improving replanning success rate and trajectory smoothness while reducing computational overhead.