A Local Consistency Algorithm to Shorten the Convergence Time and Improve the Robustness of Self-propelled Swarms