Skip to main content
SpringerLink
Log in

Scalable Optimal Formation Path Planning for Multiple Interconnected Robots via Convex Polygon Trees

  • Regular paper
  • Published:
Journal of Intelligent & Robotic Systems Aims and scope Submit manuscript

Abstract

A Reconfigurable Modular Robotic System (RMRS) consists of multiple interconnected robots and can achieve various functionalities by rearranging its modular robots, such as transporting loads of various shapes. The path planning for an RMRS involves the system motion and also its formation arrangements. Sampling-based path planning for the RMRS might be inefficient due to the formation variety. Recently, convex subsets of the obstacle-free workspace, referred to as polygon nodes, are instead sampled to formulate constrained optimization problems. The success rate of sampling is however unsatisfactory due to connectivity requirements. This paper proposes an obstacle-aware mixture density network to guide the generation of polygon nodes, where the connectivity of polygon nodes is guaranteed by non-zero Minkowski differences between the formation geometry and the intersection of nodes. Subsequently, Convex-Polygon Trees* (CPTs*) are proposed to connect these polygon nodes in an RRT* manner, outputting candidates of convex optimization problems. The optimality degeneration due to distance approximation is proven bounded and the computational complexity is shown linear to the Lebesgue measure of the entire workspace space. Numerical simulations have shown that in most tested large and cluttered environments the CPT* is more than 8 times faster than an existing constrained optimization method. The results have also shown CPT*’ improved scalability to large environments and enhanced efficiency in dealing with narrow passages.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

Abbreviations

\(\mu _i\), \(\sigma _i\), \(\pi _i\) :

Parameters of the ith Gaussian

\(\omega \) :

Subset of workspace covered by a sliding window

\(\omega _{free}\) :

Subset of obstacle-free workspace covered by a sliding window

\(\phi \) :

Formation path

\(\Phi ^*\) :

Optimal polygon path

\(\phi ^*\) :

Optimal (sub-optimal) formation path

\(\delta \) :

Minimum distance from the formations along this path to the obstacles

e :

Encoding of obstacles in \(\omega \)

O :

Point clouds of the obstacles within \(\omega \)

q :

RMRS configuration

\({\textbf {q}}_0\) :

Initial RMRS configuration

s :

Aggregated RMRS state

u :

Expansion direction

x :

Sampled position

\({\textbf {x}}_{new}\),\(\mathcal {P}_{new}\) :

Newly sampled position, newly sampled polygon node

Z \((\mathcal {P})\) :

Vectorized representation of \(\mathcal {P}\)

\(\mathcal {A}_i\) :

Geometry of the ith formation

\(\mathcal {A}_i({\textbf {q}}), \mathcal {A}(s)\) :

Occupancy of RMRS in the workspace under Formation i at configuration q (i.e., at state s)

\(\mathcal {B}_i\) :

The ith obstacle

\(\mathcal {C}\) :

Configuration space

\(\mathcal {G}\) :

Set of goal formation states

\(\mathcal {I}_\Phi \) :

Index set of the polygon nodes in \(\Phi \)

\(\mathcal {I}_\textit{A}\), \(\mathcal {I}_\textit{B}\) :

Index set of formation arrangments, obstacle index set

\(\mathcal {P}\),\(\mathcal {P}_\textit{i}\) :

Sampled polygon (polygon node), the i sampled polygon

\(\mathcal {P}_0\),\(\mathcal {I}_\textit{G}\) :

Polygon node containing initial formation state, Polygon node containing goal formation state

\(\mathcal {P}_\textit{i}\),\(\textit{i}+1\) :

Intersection of \(\mathcal {P}_\textit{i}\) and \(\mathcal {P}_\textit{i}+1\)

\(\mathcal {S}\) :

Aggregated RMRS state space

\(\mathcal {W}\) :

Workspace

\(\mathcal {W}_{free}\) :

Obstacle-free workspace

\(\pi \) :

Homotopic transform

I :

Formation variable

\({\varvec{I}}_0\), \({\varvec{I}}_g\) :

Initial formation arrangement, goal formation arrangement

\(\textit{J}_\Phi \) :

Cost associated with the polygon path \(\Phi \)

References

  1. Alonso-Mora, J., Baker, S., Rus, D.: Multi-robot formation control and object transport in dynamic environments via constrained optimization. The International Journal of Robotics Research 36(9), 1000–1021 (2017)

    Article  Google Scholar 

  2. Anderson, P., et al.: Vision-and-language navigation: Interpreting visually-grounded navigation instructions in real environments. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pp 3674–3683 (2018)

  3. Antonelli, G., Antonelli, G.: Underwater robots, vol 3. Springer (2014)

  4. Arnström, D., Axehill, D.: A unifying complexity certification framework for active-set methods for convex quadratic programming. IEEE Trans. Autom. Control 67(6), 2758–2770 (2021)

    Article  MathSciNet  MATH  Google Scholar 

  5. Bai, X., Yan, W., Cao, M., et al.: Distributed multi-vehicle task assignment in a time-invariant drift field with obstacles. IET Control Theory & Applications 13(17), 2886–2893 (2019)

    Article  MathSciNet  Google Scholar 

  6. Banino, A., et al.: Vector-based navigation using grid-like representations in artificial agents. Nature 557(7705), 429 (2018)

    Article  Google Scholar 

  7. Barfoot, T.D., Clark, C.M.: Motion planning for formations of mobile robots. Robot. Auton. Syst. 46(2), 65–78 (2004)

    Article  Google Scholar 

  8. Boroujeni, Z., Goehring, D., Ulbrich, F., et al.: Flexible unit a-star trajectory planning for autonomous vehicles on structured road maps. In: 2017 IEEE international conference on vehicular electronics and safety (ICVES), IEEE, pp 7–12 (2017)

  9. Cheng, P., LaValle, SM.: Resolution complete rapidly-exploring random trees. In: Proceedings 2002 IEEE international conference on robotics and automation (cat. no. 02CH37292), IEEE, pp 267–272 (2002)

  10. Chyba, M., Cazzaro, D., Invernizzi L., et al.: Trajectory design for autonomous underwater vehicles for basin exploration. In: 9 th International Conference on Computer and IT Applications in the Maritime Industries, pp 139–151 (2010)

  11. Davey, J., Kwok, N., Yim, M.: Emulating self-reconfigurable robots-design of the smores system. In: 2012 IEEE/RSJ international conference on intelligent robots and systems, IEEE, pp 4464–4469 (2012)

  12. Deits, R., Tedrake, R.: Computing large convex regions of obstacle-free space through semidefinite programming. In: Algorithmic foundations of robotics XI. Springer, p 109–124 (2015)

  13. Fung, N., Rogers, J., Nieto, C., et al.: Coordinating multi-robot systems through environment partitioning for adaptive informative sampling. In: 2019 International Conference on Robotics and Automation (ICRA), IEEE, pp 3231–3237 (2019)

  14. Furno, L., Blanke, M., Galeazzi, R., et al.: Self-reconfiguration of modular underwater robots using an energy heuristic. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, pp 6277–6284 (2017)

  15. Gill, P.E., Murray, W., Saunders, M.A.: Snopt: An sqp algorithm for large-scale constrained optimization. SIAM Rev. 47(1), 99–131 (2005)

    Article  MathSciNet  MATH  Google Scholar 

  16. Girdhar, R., Fouhey, D.F., Rodriguez, M., et al.: Learning a predictable and generative vector representation for objects. In: European Conference on Computer Vision, Springer, pp 484–499 (2016)

  17. Holleman, C., Kavraki, L.E.: A framework for using the workspace medial axis in prm planners. In: Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), IEEE, pp 1408–1413 (2000)

  18. Hsu, D., Jiang, T., Reif, J., et al.: The bridge test for sampling narrow passages with probabilistic roadmap planners. In: 2003 IEEE international conference on robotics and automation (cat. no. 03CH37422), IEEE, pp 4420–4426 (2003)

  19. Ichter, B., Harrison, J., Pavone, M.: Learning sampling distributions for robot motion planning. In: 2018 IEEE International Conference on Robotics and Automation (ICRA), IEEE, pp 7087–7094 (2018)

  20. Karaman, S., Frazzoli, E.: Sampling-based algorithms for optimal motion planning. The international journal of robotics research 30(7), 846–894 (2011)

    Article  MATH  Google Scholar 

  21. Ko, I., Kim, B., Park, F.C.: Vf-rrt: Introducing optimization into randomized motion planning. In: 2013 9th Asian Control Conference (ASCC), IEEE, pp 1–5 (2013)

  22. Koul, A., Greydanus, S., Fern, A.: Learning finite state representations of recurrent policy networks.(2018) arXiv:1811.12530

  23. Li, H., Liu, Y., Ouyang, W., et al.: Zoom out-and-in network with map attention decision for region proposal and object detection. Int. J. Comput. Vision 127(3), 225–238 (2019)

    Article  Google Scholar 

  24. Lu, W., Liu, D.: A scalable sampling-based optimal path planning approach via search space reduction. IEEE Access 7:153,921–153,935 (2019)

  25. Mellinger, D., Kushleyev, A., Kumar, V.: Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams. In: 2012 IEEE international conference on robotics and automation, IEEE, pp 477–483 (2012)

  26. Pan, S.J., Yang, Q.: A survey on transfer learning. IEEE Trans. Knowl. Data Eng. 22(10), 1345–1359 (2009)

    Article  Google Scholar 

  27. Peck, R.H., Timmis, J., Tyrrell, A.M.: Omni-pi-tent: An omnidirectional modular robot with genderless docking. In: Annual Conference Towards Autonomous Robotic Systems, Springer, pp 307–318 (2019)

  28. Qureshi, A.H., Simeonov, A., Bency, M.J., et al.: Motion planning networks. In: 2019 International Conference on Robotics and Automation (ICRA), IEEE, pp 2118–2124 (2019)

  29. Shome, R., Solovey, K., Dobson, A., et al.: drrt*: Scalable and informed asymptotically-optimal multi-robot motion planning. Auton. Robot. 44(3), 443–467 (2020)

    Article  Google Scholar 

  30. Solana, Y., Furci, M., Cortés, J., et al.: Multi-robot path planning with maintenance of generalized connectivity. In: 2017 International Symposium on Multi-Robot and Multi-Agent Systems (MRS), IEEE, pp 63–70 (2017)

  31. Swingler, A., Ferrari, S.: A cell decomposition approach to cooperative path planning and collision avoidance via disjunctive programming. In: 49th IEEE Conference on Decision and Control (CDC), IEEE, pp 6329–6336 (2010)

  32. Toh, K.C., Todd, M.J., Tütüncü, R.H.: Sdpt3-a matlab software package for semidefinite programming, version 1.3. Optimization methods and software 11(1-4):545–581 (1999)

  33. Wei, H., Lu, W., Ferrari, S.: An information value function for nonparametric gaussian processes. In: Proceedings of Neural Information Processing Systems Conference, NIPS, Lake Tahoe, NV (2012)

  34. Wei, H., Lu, W., Zhu, P., et al.: Visibility-based motion planning for active target tracking and localization. In: IROS, Chicago, IL, USA (2014)

  35. Woolfrey, J., Lu, W., Vidal-Calleja, T., et al.: Clarifying clairvoyance: Analysis of forecasting models for near-sinusoidal periodic motion as applied to auvs in shallow bathymetry. Ocean Engineering 190:106,385 (2019)

Download references

Acknowledgements

This work is supported by the National Natural Science Foundation of China #62003110 and the Shenzhen Science and Technology Innovation Foundation #JCYJ20210324132607018, #JSGG20210420091804012, and #GXWD20220811163649003

Author information

Authors and Affiliations

  1. Guangdong Provincial Key Laboratory of Intelligent Morphing Mechanisms and Adaptive Robotics, School of Mechanical Engineering and Automation, Harbin Institute of Technology (Shenzhen), Guangdong, China

    Wenjie Lu, Hao Xiong, Zhengjie Zhang, Zhezhe Hu & Tianming Wang

Authors
  1. Wenjie Lu
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Hao Xiong
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Zhengjie Zhang
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Zhezhe Hu
    View author publications

    You can also search for this author in PubMed Google Scholar

  5. Tianming Wang
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding authors

Correspondence to Wenjie Lu or Hao Xiong.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Springer Nature or its licensor (e.g. a society or other partner) holds exclusive rights to this article under a publishing agreement with the author(s) or other rightsholder(s); author self-archiving of the accepted manuscript version of this article is solely governed by the terms of such publishing agreement and applicable law.

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Lu, W., Xiong, H., Zhang, Z. et al. Scalable Optimal Formation Path Planning for Multiple Interconnected Robots via Convex Polygon Trees. J Intell Robot Syst 109, 63 (2023). https://doi.org/10.1007/s10846-023-01994-0

Download citation

  • Received:

  • Accepted:

  • Published:

  • DOI: https://doi.org/10.1007/s10846-023-01994-0

Keywords

Search

Navigation