Hierarchical formation navigation framework for multi-vehicle systems with safety-critical obstacle avoidance in cluttered environments

Formation navigation in cluttered environments presents significant challenges due to the need to simultaneously maintain formation structure and ensure safe obstacle avoidance. This work proposes a hierarchical formation navigation framework for multi-vehicle systems operating in safety-critical environments with both static and dynamic obstacles. The framework consists of an upper-layer centralized global planner and a lower-layer distributed real-time local planner. At the global level, a formation-level sampling-based algorithm is developed by enhancing RRT* with a Quick Rewire strategy, enabling smooth and collision-free reference trajectories while preserving geometric formation constraints. A differentiable formulation based on signed distance functions and Log-Sum-Exp approximation is introduced to model obstacle avoidance constraints for irregular polygonal obstacles, improving optimization smoothness and adaptability. At the local level, a distributed nonlinear model predictive control (NMPC) framework incorporating discrete-time control barrier functions ensures anticipatory and robust dynamic obstacle avoidance. An assumed-states mechanism is adopted to decouple inter-vehicle constraints, allowing real-time and fully parallel trajectory generation. The proposed approach is comprehensively validated through general simulations in cluttered static environments, dynamic scenario evaluations in the CARLA simulator, and real-world experiments under realistic perception and control delays, demonstrating its effectiveness, robustness, and practical applicability in physical deployment.