https://github.com/RoyaFiroozi/Centralized-Planning.
Advances in vehicular communication technologies are expected to facilitate cooperative driving in the future. Connected and Automated Vehicles (CAVs) are able to collaboratively plan and execute driving maneuvers by sharing their perceptual knowledge and future plans. In this paper, we present an architecture for autonomous navigation of tight multi-lane platoons travelling on public roads. Using the proposed approach, CAVs are able to form single or multi-lane platoons of various geometrical configurations. They are able to reshape and adjust their configurations according to changes in the environment. The proposed architecture consists of three main components: an online decision-maker, an offline motion planner and an online path-follower. The decision-maker selects the desired platoon configuration based on real-time information about the surrounding traffic. The motion planner uses an optimization-based approach for cooperative formation and reconfiguration in tight spaces. The motion planner uses a Model Predictive Control scheme to plan smooth, dynamically feasible and collision-free trajectories for all the vehicles within the platoon. The paper addresses online computation limitations by employing a family of maneuvers pre-computed offline and stored on the vehicles' control units to be executed by a low-level path-following feedback controller in real-time based on the selected desired configuration. We demonstrate the effectiveness of our approach through simulations of three case studies: 1) formation reconfiguration 2) obstacle avoidance, and 3) bench-marking against behavior-based planning in which the desired formation is achieved using a sequence of motion primitives. Videos and software can be found online here