Journal of Intelligent & Robotic Systems

, Volume 80, Issue 3–4, pp 507–523 | Cite as

3D Robot Formations Path Planning with Fast Marching Square

  • David ÁlvarezEmail author
  • Javier V. Gómez
  • Santiago Garrido
  • Luis Moreno


This work presents a path planning algorithm for 3D robot formations based on the standard Fast Marching Square (FM2) path planning method. This method is enlarged in order to apply it to robot formations motion planning. The algorithm is based on a leader-followers scheme, which means that the reference pose for the follower robots is defined by geometric equations that place the goal pose of each follower as a function of the leader’s pose. Besides, the Frenet-Serret frame is used to control the orientation of the formation. The algorithm presented allows the formation to adapt its shape so that the obstacles are avoided. Additionally, an approach to model mobile obstacles in a 3D environment is described. This model modifies the information used by the FM2 algorithm in favour of the robots to be able to avoid obstacles. The shape deformation scheme allows to easily change the behaviour of the formation. Finally, simulations are performed in different scenarios and a quantitative analysis of the results has been carried out. The tests show that the proposed shape deformation method, in combination with the FM2 path planner, is robust enough to manage autonomous movements through an indoor 3D environment.


Robot formations motion planning Formation control Fast marching square 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Martin, M., Klupar, P., Kilberg, S., Winter, J.: TechSat 21 and revolutionizing space missions using microsatellites. In: AIAA/USU Conference on Small Satellites (2001)Google Scholar
  2. 2.
    Dewan, A., Mahendran, A., Soni, N., Krishna, K.M.: Heterogeneous UGV-MAV exploration using integer programming. In: AIAA/USU Conference on Small Satellites on Intelligent Robots and Systems (2013)Google Scholar
  3. 3.
    Hauert, S., Zufferey, J.C., Floreano, D.: Reverse-engineering of artificially evolved controllers for swarms of robots. In: IEEE Congress on Evolutionary Computation (2009)Google Scholar
  4. 4.
    Acevedo, J.J., Arrue, B.C., Maza, I., Ollero, A.: Cooperative large area surveillance with a team of aerial mobile robots for long endurance missions. J. Intell. Robot. Syst. 70(1–4), 329–345 (2013)CrossRefGoogle Scholar
  5. 5.
    Likhachev, M., Keller, J., Kumar, V., Dobrokhodov, V., Jones, K., Wurz, J., Kaminer, I.: Planning for opportunistic surveillance with multiple robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5750–5757. Tokyo (2013)Google Scholar
  6. 6.
    Bouabdallah, S.: Design and Control of Quadrotors with Applicationto Autonomous Flying, Theses 3727. École polytechnique fédérale de Lausanne (2007)Google Scholar
  7. 7.
    Hrabar, S.: Reactive obstacle avoidance for rotorcraft UAVs. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4967–4974. San Francisco (2011)Google Scholar
  8. 8.
    Shen, S., Michael, N., Kumar, V.: 3D estimation and control for autonomous flight with constrained computation. In: IEEE International Conference of Robotics and Automation. Shanghai (2011)Google Scholar
  9. 9.
    Hino, T.: Simple formation control scheme tolerant to communication failures for small unmanned air vehicles. In: International Congress of the Aeronautical Sciences. Nice (2010)Google Scholar
  10. 10.
    Balch, T., Arkin, R.C.: Behaviour-based formation control for multi-robot teams. IEEE Trans. Robot. Autom. 14(6), 926–939 (1998)CrossRefGoogle Scholar
  11. 11.
    Naffin, D., Sukhatme, G.: Negotiated formations. In: Proceedings of the International Conference on Intelligent Autonomous Systems, pp. 181–190. Amsterdam (2004)Google Scholar
  12. 12.
    Fredslund, J., Matari, M.J.: A general algorithm for robot formations using local sensing and minimal communication. IEEE Trans. Robot. Autom. 18(5), 837–846 (2002)CrossRefGoogle Scholar
  13. 13.
    Lemay, M, Michaud, F., Létourneau, D., Valin, J.-M.: Autonomous initialization of robot formations. In: Proceedings of the IEEE International Conference on Robotics and Automation, vol. 3, pp. 3018–3023. New Orleans (2004)Google Scholar
  14. 14.
    Ogren, P., Egerstedt, M., Hu, X.: A control Lyapunov function approach to multiagent coordination. IEEE Trans. Robot. Autom. 18(5), 847–851 (2002)CrossRefGoogle Scholar
  15. 15.
    Zhang, M., Shen, Y., Wang, Q., Wang, Y.: Dynamic artificial potential field based multi-robot formation control. In: IEEE Instrumentation and Measurement Technology Conference, pp. 1530–1534. Austin (2010)Google Scholar
  16. 16.
    Cao, Z., Xie, L., Zhang, B., Wang, S., Tan, M.: Formation constrained multi-robot system in unknown environments. Proceedings Conference on Robotics and Automation, vol. 1, pp. 735–740 (2003)Google Scholar
  17. 17.
    Tan, K.H., Lewis, M.A.: Virtual structures for high-precision cooperative mobile robotic control. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 1, pp. 132–139 (1996)Google Scholar
  18. 18.
    Ren, W., Beard, R.W.: Decentralized scheme for spacecraft formation flying via the virtual structure approach. AIAA J. Guid. Control. Dyn. 1(1), 73–82 (2004)CrossRefGoogle Scholar
  19. 19.
    Ahmad, A., Nascimento, T., Conceiçao, A.G.S., Moreira, A.P., Lima, P.: Perception-driven multi-robot formation control. In: IEEE International Conference on Robotics and Automation, pp. 1851–1856. Karlsruhe (2013)Google Scholar
  20. 20.
    Kanjanawanishkul, K., Zell, A.: A model-predictive approach to formation control of omnidirectional mobile robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2771–2776. Nice (2008)Google Scholar
  21. 21.
    Álvarez, D., Gómez, J.V., Garrido, S., Moreno, L.: 3D Robot formations planning with fast marching square. In: IEEE International Conference on Autonomous Robot Systems and Competitions. Espinho (2014)Google Scholar
  22. 22.
    Yu, W., Chen, G., Cao, M.: Distributed leader-follower flocking control for multi-agent dynamical systems with time-varying velocities. Syst. Control Lett. 59(9), 543–552 (2010)zbMATHMathSciNetCrossRefGoogle Scholar
  23. 23.
    Garrido, S., Moreno, L., Lima, P.: Robot formation motion planning using fast marching. J. Robot. Auton. Syst. 59(9), 675–683 (2011)CrossRefGoogle Scholar
  24. 24.
    Álvarez, D., Lumbier, A., Gómez, J.V., Garrido, S., Moreno, L.: Precision grasp planning with Gifu hand III based on fast marching square. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4549–4554. Tokyo (2013)Google Scholar
  25. 25.
    Valero, A., Gómez, J.V., Garrido, S., Moreno, L.: The path to efficiency. IEEE Robot. Autom. Mag. 20(4), 111–120 (2013)CrossRefGoogle Scholar
  26. 26.
    Gómez, J.V., Lumbier, A., Garrido, S., Moreno, L.: Planning Robot Formations with Fast Marching Square Including Uncertainty Conditions. J. Robot. Auton. Syst. 61(2), 137–152 (2013)CrossRefGoogle Scholar
  27. 27.
    Sethian, J.A.: A fast marching level set method for monotonically advancing fronts. In: Proceedings National Academy of Science, vol. 93, pp. 1591–1595 (1996)Google Scholar
  28. 28.
    Osher, S., Sethian, J.A.: Fronts propagating with curvature-dependent speed: Algorithms based on Hamilton-Jacobi formulations. J. Comput. Phys. 79, 12–49 (1988)zbMATHMathSciNetCrossRefGoogle Scholar
  29. 29.
    Garrido, S., Moreno, L., Abderrahim, M., Blanco, D.: FM2: A Real-time Sensor-based Feedback Controller for Mobile Robots. Intl. J. Robot. Autom. 24(1), 3169–3192 (2009)Google Scholar
  30. 30.
    Yatziv, L., Bartesaghi, A., Sapiro, G.: A fast O(n) implementation of the fast marching algorithm. J. Comput. Phys. 212(1), 393–399 (2005)Google Scholar
  31. 31.
    Garrido, S., Moreno, L., Gómez, J.V., Lima, P.U.: General path planning methodology for leader-followers based robot formations. Intl. J. Adv. Robot. Syst. 10(64), 1–10 (2013)CrossRefGoogle Scholar
  32. 32.
    Gómez, J.V., Garrido, S., Moreno, L.: Adaptive robot formations using fast marching square working under uncertainty conditions. In: IEEE Workshop on Advanced Robotics and Its Social Impacts, pp. 68–71. California (2011)Google Scholar
  33. 33.
    Frenet, F.: Sur les Courbes à Double Courbure. Journal de Mathématiques Pures et Appliquées 1(17), 437–447 (1852)Google Scholar
  34. 34.
    Serret, J.A.: Sur Quelques Formules Relatives à la Théorie des Courbes à Double Courbure. Journal de Mathématiques Pures et Appliquées 1(16), 193–207 (1851)Google Scholar
  35. 35.
    Dalal, N., Triggs, B.: Histograms of oriented gradients for human detection. In: IEEE Computer Society Conference on Computer Vision and Pattern Recognition, vol. 1, pp. 886–893. San Diego (2005)Google Scholar
  36. 36.
    Felzenszwalb, P., Girshick, R., McAllester, D., Ramanan, D.: Object detection with discriminatively trained part-based models. IEEE Trans. Pattern. Anal. Mach. Intell. 32(9), 1627–1645 (2009)CrossRefGoogle Scholar
  37. 37.
    Volpe, R., Khosla, P.: Manipulator control with superquadric artificial potential functions: Theory and experiments. IEEE Trans. Syst. Man Cybern. 20, 1423–1436 (1990)CrossRefGoogle Scholar
  38. 38.
    El-Khoury, S., Sahbani, A.: A new strategy combining empirical and analytical approaches for grasping unknown 3d objects. J. Robot. Auton. Syst. 58(5), 497–507 (2010)CrossRefGoogle Scholar

Copyright information

© Springer Science+Business Media Dordrecht 2015

Authors and Affiliations

  • David Álvarez
    • 1
    Email author
  • Javier V. Gómez
    • 1
  • Santiago Garrido
    • 1
  • Luis Moreno
    • 1
  1. 1.Robotics Lab, Department of Systems and AutomationUniversity Carlos III of MadridLeganésSpain

Personalised recommendations