A Localized Autonomous Control Algorithm For Robots With Heterogeneous Capabilities In A Multi-Tier Architecture