Journal of Intelligent & Robotic Systems

, Volume 85, Issue 2, pp 385–408 | Cite as

An Optimization Based Approach for Relative Localization and Relative Tracking Control in Multi-Robot Systems

  • Mohamed W. MehrezEmail author
  • George K. I. Mann
  • Raymond G. Gosine


In this paper, an optimization based method is used for relative localization and relative trajectory tracking control in Multi-Robot Systems (MRS’s). In this framework, one or more robots are located and commanded to follow time varying trajectories with respect to another (possibly moving) robot reference frame. Such systems are suitable for a considerable number of applications, e.g. patrolling missions, searching operations, perimeter surveillance, and area coverage. Here, the nonlinear and constrained motion and measurement models in an MRS are incorporated to achieve an accurate state estimation algorithm based on nonlinear Moving Horizon Estimation (MHE) and a tracking control method based on Nonlinear Model Predictive Control (NMPC). In order to fulfill the real-time requirements, a fast and efficient algorithm based on a Real Time Iteration (RTI) scheme and automatic C-code generation, is adopted. Numerical simulations are conducted to: first, compare the performance of MHE against the traditional estimator used for relative localization, i.e. extended Kalman filter (EKF); second, evaluate the utilized relative localization and tracking control algorithm when applied to a team of multiple robots; finally, laboratory experiments are performed, for real-time performance evaluation. The conducted simulations validated the adopted algorithm and the experiments demonstrated its practical applicability.


Multi-robot systems Relative localization Relative trajectory tracking control Fast MHE and NMPC Code generation 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Acevedo, J., Arrue, B., 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
  2. 2.
    Acevedo, J., Arrue, B., Maza, I., Ollero, A.: Distributed approach for coverage and patrolling missions with a team of heterogeneous aerial robots under communication constraints. Int. J. Adv. Robot. Syst. 10(28), 1–13 (2013)Google Scholar
  3. 3.
    Adept mobilerobots: Pioneer-3at operation manual. Accessed: 2016-03-18
  4. 4.
    Alessandri, A., Awawdeh, M.: Moving-Horizon Estimation for Discrete-Time Linear Systems with Measurements Subject to Outliers. In: Decision and Control (CDC), 2014 IEEE 53Rd Annual Conference On, pp 2591–2596 (2014)Google Scholar
  5. 5.
    Antonelli, G., Arrichiello, F., Chiaverini, S.: Flocking for Multi-Robot Systems via the Null-Space-Based Behavioral Control. In: Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference On, pp 1409–1414 (2008)Google Scholar
  6. 6.
    Awawdeh, M.J.: Moving-Horizon Estimation for Outliers Detection and Data Mining Applications. Ph.D. thesis, Universita degli Studi di Genova (2015)Google Scholar
  7. 7.
    Balch, T., Arkin, R.: Behavior-based formation control for multirobot teams. IEEE Trans. Robot. Autom. 14(6), 926–939 (1998)CrossRefGoogle Scholar
  8. 8.
    Balch, T., Hybinette, M.: Social Potentials for Scalable Multi-Robot Formations. In: Robotics and Automation, 2000. Proceedings. ICRA ’00. IEEE International Conference On. 1, vol. 1, pp 73–80 (2000)Google Scholar
  9. 9.
    Bar-Shalom, Y., Kirubarajan, T., Li, X.R.: Estimation with Applications to Tracking and Navigation. John Wiley & Sons, Inc., New York (2002)Google Scholar
  10. 10.
    Beard, R., McLain, T., Nelson, D., Kingston, D., Johanson, D.: Decentralized cooperative aerial surveillance using fixed-wing miniature uavs. Proc. IEEE 94(7), 1306–1324 (2006)CrossRefGoogle Scholar
  11. 11.
    Bernard, M., Kondak, K., Maza, I., Ollero, A.: Autonomous transportation and deployment with aerial robots for search and rescue missions. J. Field Rob. 28(6), 914–931 (2011)CrossRefGoogle Scholar
  12. 12.
    Carlone, L., Kaouk Ng, M., Du, J., Bona, B., Indri, M.: Simultaneous localization and mapping using rao-blackwellized particle filters in multi robot systems. J. Intell. Robot. Syst. 63(2), 283–307 (2011)CrossRefGoogle Scholar
  13. 13.
    Cognetti, M., Stegagno, P., Franchi, A., Oriolo, G., Bulthoff, H.: 3-D Mutual Localization with Anonymous Bearing Measurements. In: Robotics and Automation (ICRA), 2012 IEEE International Conference On, pp 791–798 (2012)Google Scholar
  14. 14.
    De Silva, O., Mann, G., Gosine, R.: Development of a Relative Localization Scheme for Ground-Aerial Multi-Robot Systems. In: Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference On, pp 870–875 (2012)Google Scholar
  15. 15.
    Diehl, M., Bock, H., Schlder, J.P., Findeisen, R., Nagy, Z., Allgwer, F.: Real-time optimization and nonlinear model predictive control of processes governed by differential-algebraic equations. J. Process Control 12(4), 577–585 (2002)CrossRefGoogle Scholar
  16. 16.
    Dunbar, W., Murray, R.: Model Predictive Control of Coordinated Multi-Vehicle Formations. In: Decision and Control, 2002, Proceedings of the 41St IEEE Conference On. 4, vol. 4, pp 4631–4636 (2002)Google Scholar
  17. 17.
    Fenwick, J., Newman, P., Leonard, J.: Cooperative Concurrent Mapping and Localization. In: Robotics and Automation, 2002. Proceedings. ICRA ’02. IEEE International Conference On, vol. 2, pp 1810–1817 (2002)Google Scholar
  18. 18.
    Ferreau, H., Kraus, T., Vukov, M., Saeys, W., Diehl, M.: High-Speed Moving Horizon Estimation Based on Automatic Code Generation. In: Decision and Control (CDC), 2012 IEEE 51St Annual Conference On, pp 687–692 (2012)Google Scholar
  19. 19.
    Grüne, L., Pannek, J.: Nonlinear Model Predictive Control: Theory and Algorithms. Communications and Control Engineering. Springer, Heidelberg (2011)CrossRefzbMATHGoogle Scholar
  20. 20.
    Gu, D., Hu, H.: A model predictive controller for robots to follow a virtual leader. Robotica 27, 905–913 (2009)CrossRefGoogle Scholar
  21. 21.
    Hazon, N., Mieli, F., Kaminka, G.: Towards Robust On-Line Multi-Robot Coverage. In: Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference On, pp 1710–1715 (2006)Google Scholar
  22. 22.
    Houska, B., Ferreau, H., Diehl, M.: ACADO Toolkit – An Open Source Framework for Automatic Control and Dynamic Optimization. Optim. Control Appl. Methods 32(3), 298–312 (2011)MathSciNetCrossRefzbMATHGoogle Scholar
  23. 23.
    Houska, B., Ferreau, H.J., Diehl, M.: An auto-generated real-time iteration algorithm for nonlinear {MPC} in the microsecond range. Automatica 47(10), 2279–2285 (2011)MathSciNetCrossRefzbMATHGoogle Scholar
  24. 24.
    Hsieh, M.A., Cowley, A., Keller, J.F., Chaimowicz, L., Grocholsky, B., Kumar, V., Taylor, C.J., Endo, Y., Arkin, R.C., Jung, B., Wolf, D.F., Sukhatme, G.S., MacKenzie, D.C.: Adaptive teams of autonomous aerial and ground robots for situational awareness. J. Field Rob. 24(11-12), 991–1014 (2007)CrossRefGoogle Scholar
  25. 25.
    Kanjanawanishkul, K.: Coordinated Path Following Control and Formation Control of Mobile Robots. Ph.D. thesis, University of Tübingen (2010)Google Scholar
  26. 26.
    Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Distributed nonlinear model predictive control of an autonomous tractortrailer system. Mechatronics 24(8), 926–933 (2014)CrossRefGoogle Scholar
  27. 27.
    Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Learning in centralized nonlinear model predictive control: Application to an autonomous tractor-trailer system. IEEE Trans. Control Syst. Technol. 23(1), 197–205 (2015)CrossRefGoogle Scholar
  28. 28.
    Kayacan, E., Kayacan, E., Ramon, H., Saeys, W.: Robust tube-based decentralized nonlinear model predictive control of an autonomous tractor-trailer system. IEEE/ASME Trans. Mechatron. 20(1), 447–456 (2015)CrossRefGoogle Scholar
  29. 29.
    Kingston, D., Beard, R., Holt, R.: Decentralized perimeter surveillance using a team of uavs. IEEE Trans. Robot. 24(6), 1394–1404 (2008)CrossRefGoogle Scholar
  30. 30.
    Lewis, M., Tan, K.H.: High precision formation control of mobile robots using virtual structures. Auton. Robot. 4(4), 387–403 (1997)CrossRefGoogle Scholar
  31. 31.
    Li, X., Xiao, J., Cai, Z.: Backstepping Based Multiple Mobile Robots Formation Control. In: Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference On, pp 887–892 (2005)Google Scholar
  32. 32.
    Mariottini, G., Morbidi, F., Prattichizzo, D., Pappas, G., Daniilidis, K.: Leader-Follower Formations: Uncalibrated Vision-Based Localization and Control. In: Robotics and Automation, 2007 IEEE International Conference On, pp 2403–2408 (2007)Google Scholar
  33. 33.
    Maza, I., Ollero, A.: Multiple Uav Cooperative Searching Operation Using Polygon Area Decomposition and Efficient Coverage Algorithms. In: Alami, R., Chatila, R., Asama, H. (eds.) Distributed Autonomous Robotic Systems 6. Birkhuser Basel, pp 221–230 (2007)Google Scholar
  34. 34.
    Mehrez, M.W., Mann, G.K., Gosine, R.G.: Stabilizing Nmpc of Wheeled Mobile Robots Using Open-Source Real-Time Software. In: Advanced Robotics (ICAR), 2013 16Th International Conference On, pp 1–6 (2013)Google Scholar
  35. 35.
    Mehrez, M.W., Mann, G.K., Gosine, R.G.: Formation Stabilization of Nonholonomic Robots Using Nonlinear Model Predictive Control. In: Electrical and Computer Engineering (CCECE), 2014 IEEE 27Th Canadian Conference On, pp 1–6 (2014)Google Scholar
  36. 36.
    Mehrez, M.W., Mann, G.K., Gosine, R.G.: Nonlinear Moving Horizon State Estimation for Multi-Robot Relative Localization. In: Electrical and Computer Engineering (CCECE), 2014 IEEE 27Th Canadian Conference On, pp 1–5 (2014)Google Scholar
  37. 37.
    Nascimento, T.P., Moreira, A.P., Conceio, A.G.S.: Multi-robot nonlinear model predictive formation control: Moving target and target absence. Robot. Auton. Syst. 61(12), 1502–1515 (2013)CrossRefGoogle Scholar
  38. 38.
    Pathiranage, C., Watanabe, K., Jayasekara, B., Izumi, K.: Simultaneous Localization and Mapping: a Pseudolinear Kalman Filter (Plkf) Approach. In: Information and Automation for Sustainability, 2008. ICIAFS 2008. 4Th International Conference On, pp 61–66 (2008)Google Scholar
  39. 39.
    Rao, C., Rawlings, J.: Nonlinear Moving Horizon State Estimation. In: Allg?wer, F., Zheng, A. (eds.) Nonlinear Model Predictive Control, Progress in Systems and Control Theory, vol. 26, pp 45–69. Birkhuser Basel (2000)Google Scholar
  40. 40.
    Rawlings, J.B., Bakshi, B.R.: Particle filtering and moving horizon estimation. Comput. Chem. Eng. 30(1012), 1529–1541 (2006)CrossRefGoogle Scholar
  41. 41.
    Rivard, F., Bisson, J., Michaud, F., Letourneau, D.: Ultrasonic Relative Positioning for Multi-Robot Systems. In: Robotics and Automation, 2008. ICRA 2008. IEEE International Conference On, pp 323–328 (2008)Google Scholar
  42. 42.
    Saffarian, M., Fahimi, F.: Non-iterative nonlinear model predictive approach applied to the control of helicopters group formation. Robot. Auton. Syst. 57(67), 749–757 (2009)CrossRefzbMATHGoogle Scholar
  43. 43.
    Sanchez, J., Fierro, R.: Sliding Mode Control for Robot Formations. In: Intelligent Control. 2003 IEEE International Symposium On, pp 438–443 (2003)Google Scholar
  44. 44.
    Silva, O.D., Mann, G.K.I., Gosine, R.G.: An ultrasonic and vision-based relative positioning sensor for multirobot localization. IEEE Sensors J. 15(3), 1716–1726 (2015)CrossRefGoogle Scholar
  45. 45.
    Thrun, S., Burgard, W., Fox, D.: Probabilistic robotics (intelligent robotics and autonomous agents) the MIT press (2005)Google Scholar
  46. 46.
    Trawny, N., Zhou, X., Zhou, K., Roumeliotis, S.: Interrobot transformations in 3-d. IEEE Trans. Robot. 26(2), 226–243 (2010)CrossRefGoogle Scholar
  47. 47.
    Vukov, M., Van Loock, W., Houska, B., Ferreau, H., Swevers, J., Diehl, M.: Experimental Validation of Nonlinear Mpc on an Overhead Crane Using Automatic Code Generation. In: American Control Conference (ACC), 2012, pp 6264–6269 (2012)Google Scholar
  48. 48.
    Wanasinghe, T., Mann, I.G., Gosine, R.: Relative localization approach for combined aerial and ground robotic system. J. Intell. Robot. Syst. 77(1), 113–133 (2015)CrossRefGoogle Scholar
  49. 49.
    Wanasinghe, T., Mann, G., Gosine, R.: Pseudo-Linear Measurement Approach for Heterogeneous Multi-Robot Relative Localization. In: Advanced Robotics (ICAR), 2013 16Th International Conference On, pp 1–6 (2013)Google Scholar
  50. 50.
    Wanasinghe, T., Mann, G., Gosine, R.: Distributed leader-assistive localization method for a heterogeneous multirobotic system. IEEE Trans. Autom. Sci. Eng. 12(3), 795–809 (2015)CrossRefGoogle Scholar
  51. 51.
    Wang, S., Chen, L., Gu, D., Hu, H.: An optimization based moving horizon estimation with application to localization of autonomous underwater vehicles. Robot. Auton. Syst. 62(10), 1581–1596 (2014)CrossRefGoogle Scholar
  52. 52.
    Worthmann, K., Mehrez, M.W., Zanon, M., Mann, G.K., Gosine, R.G., Diehl, M.: Regulation of Differential Drive Robots Using Continuous Time Mpc without Stabilizing Constraints Or Costs. In: Proceedings of the 5Th IFAC Conference on Nonlinear Model Predictive Control (NPMC15), Sevilla, Spain, pp 129–135 (2015)Google Scholar
  53. 53.
    Worthmann, K., Mehrez, M.W., Zanon, M., Mann, G.K.I., Gosine, R.G., Diehl, M.: Model predictive control of nonholonomic mobile robots without stabilizing constraints and costs. IEEE Trans. Control Syst. Technol. 24(4), 1394–1406 (2016)CrossRefGoogle Scholar
  54. 54.
    Xingxi, S., Tiesheng, W., Bo, H., Chunxia, Z.: Cooperative Multi-Robot Localization Based on Distributed Ukf. In: Computer Science and Information Technology (ICCSIT), 2010 3Rd IEEE International Conference On, vol. 6, pp 590–593 (2010)Google Scholar
  55. 55.
    Zanon, M., Frasch, J., Diehl, M.: Nonlinear Moving Horizon Estimation for Combined State and Friction Coefficient Estimation in Autonomous Driving. In: Control Conference (ECC), 2013 European, pp 4130–4135 (2013)Google Scholar
  56. 56.
    Zanon, M., Gros, S., Diehl, M.: Rotational Start-Up of Tethered Airplanes Based on Nonlinear Mpc and Mhe. In: Control Conference (ECC), 2013 European, pp 1023–1028 (2013)Google Scholar
  57. 57.
    Zanon, M., Horn, G., Gros, S., Diehl, M.: Control of Dual-Airfoil Airborne Wind Energy Systems Based on Nonlinear Mpc and Mhe. In: Control Conference (ECC), 2014 European, pp 1801–1806 (2014)Google Scholar

Copyright information

© Springer Science+Business Media Dordrecht 2016

Authors and Affiliations

  • Mohamed W. Mehrez
    • 1
    Email author
  • George K. I. Mann
    • 1
  • Raymond G. Gosine
    • 1
  1. 1.Intelligent Systems Lab, Faculty of Engineering and Applied ScienceMemorial University of NewfoundlandSt. JohnsCanada

Personalised recommendations