Safe and Efficient Control of Autonomous Vehicles in Tightly Constrained Environments