Elsevier

Robotics and Autonomous Systems

Volume 87, January 2017, Pages 38-50
Robotics and Autonomous Systems

Cooperative minimum expected length planning for robot formations in stochastic maps

https://doi.org/10.1016/j.robot.2016.09.002Get rights and content

Highlights

  • A global path planning that minimizes the expected length in a stochastic map.

  • A cooperative sensor-based local re-planning in the stochastic map.

  • A multi-vehicle consistent localization (robot pose tracking) in stochastic maps built from the multiple views of the robots.

  • A hybrid centralized–distributed architecture, using real-time wireless communication.

Abstract

This paper addresses a tightly integrated multi-robot planning, localization and navigation system in stochastic scenarios. We present a novel motion planning technique for robot formations in such kinds of environments, which computes the most likely global path in terms of a defined minimum expected length (EL). EL evaluates the expected cost of a path considering the probability of finding a non traversable zone and the cost of using an alternative traversable path. A local real time re-planning technique based on the probabilistic model is also developed for the formation when the scenario changes. The formation adapts its configuration to the shape of the free room. The partial views of all the robots are integrated to update the multi-robot localization using a modified EKF based on the measurement differencing technique which improves estimation consistency. As a result, a lower uncertainty map of the local navigation area is obtained for re-planning purposes. Experimental results, both in simulation and in real office-like settings, illustrate the performance of the described approach where a hybrid, centralized–distributed, architecture with wireless communication capabilities is used.

Introduction

The interest in the design, development and implementation of multi-vehicle systems has grown tremendously both in the fields of robotics and control systems technology due to their enhanced reliability and robustness over single-vehicle systems.

Key applications such as factory automation, surveillance, exploration or rescue missions may benefit from the use of geometrically constrained multi-robot systems, known as robot formations where close cooperation and coordination among the, possibly heterogeneous, vehicles of the team is mandatory for the successful joint mission completion. For instance, the formation can be used to escort a group of people to the exit of a building in an emergency situation where the robots in the formation structure delimit a safe area for them and are spatially arranged to provide a broader field of view to the system.

During the execution of the mission, formation control, either centralized or distributed, keeps the team members on rigid predefined relative positions whilst moving towards the goal.A well-known approach to multi-robot coordination [1] is the leader-following approach where the robot followers navigate coordinately as commanded by the robot leader to reach the mission objectives, whilst maintaining a certain geometric shape (e.g., equilateral triangle, regular pentagon, etc.), previously imposed to the formation. However, realistic scenarios with cluttered and dynamic obstacles prevent the formation from moving in the predefined shape along all its trajectory to the goal and thus the shape of the formation must be temporarily adapted to the environment.

The adaptability in the formation shape required to overcome the cluttered zones can be achieved by modeling the links between the robots as virtual spring–damper systems [2], [3]. Furthermore, this approach offers an important advantage when addressing the formation path planning problem, solved only by the robot leader, in contrast to approaches based on virtual structures [4] where paths must be computed for all team members and imply additional computational efforts to adapt their trajectories to unexpected obstacles in the environment.

In [5], the authors propose a method for planning trajectories for formations of robots under uncertainty using potential fields and a fast marching algorithm to find the optimal solution. However, the uncertainty is only applied on the relative localization of the robots with respect to the others rather than to the obstacles in the map.

The localization of a robot formation can be formulated in the general setting of multi-robot localization, where several approaches have been reported in the literature using inter-robot measurements (e.g., in absence of previous knowledge about the navigation area [6], [7], [8]), landmark observations (e.g., in presence of a grid-based [9] or feature-based map [10]) or even both of them [11]. Interestingly, EKF-based distributed approaches using inter-robot measurements aimed at real-time performance have been reported [12], [13].

During the world modeling phase, uncertainty derived from noisy sensors is incorporated into the maps used later for path planning. Thus, the algorithms applied in deterministic scenarios to compute the best paths for the robots, such as those collected in [14], are no longer valid. For path planning, it is common to assume that the state of the robot is well known and that the actions are deterministic, as opposed to the POMDP approaches for robot control. This assumption is crucial because motion or localization uncertainty makes mandatory to minimize the risk of getting lost. There are mathematically sound suboptimal planning under motion and localization uncertainty methods such as [15], using POMDPs, or [16] and [17], planning in the belief space. These solutions are, thus, finding the paths that minimizes the uncertainty of the robot localization at the goal position.

However, assuming that motion and localization are always reliable, minimizing the position uncertainty is no longer an issue. Instead, it is incorporated into the knowledge of the connectivity or traversability of the environment. This lack of deterministic information implies that only the expectation of the length of a path can be computed, based on the prior and on the observations that the robots will perform during the mission. In [18], the authors propose a graph-based algorithm to compute the expected shortest path, making the markovian assumption that the connectivity of the scenario may eventually change during the execution of the mission. Some approaches like [19], make use of the graphs created during the map-building phase to compute minimum uncertainty paths to the goal. However, even though the authors extend the graph to cover trajectories not performed during the map-building, these additional elements added do not consider the presence of obstacles. Moreover, the new potential trajectories added, as they are further away from the features in the map, present higher uncertainty levels than those trajectories used to build the map. Thus, looking for the minimum uncertainty path will never select one of those added paths.

In this work we propose a global path planning—local replanning scheme that assumes a static scenario for global path planning but that is able to deal with changes locally.

From a system architecture view point, the performance of robot formations during mission execution can be described in terms of modularity and scalability of the underlying probabilistic framework and software components, thus favoring distributed or decentralized approaches. However, from the perspective of precision, reliability and robustness, the close cooperation among the different vehicles of the robot formation, in a centralized framework, would result in a jointly coherent understanding of the navigation environment, thus reducing the uncertainty both during the localization, e.g., wider and integrated fields of view, and path planning, e.g., due to alternative paths reachable by robot followers. Therefore, a hybrid centralized–distributed approach to trade-off the advantages of both perspectives without compromising the success of the commanded mission can be the best choice at hand.

The work presented in this paper develops new techniques for global and local path planning, and extends previous work reported by the authors in localization of multi-robot systems in stochastic maps [10] and motion control for robot formations [20], which are tightly integrated in the whole system to obtain the maximum benefits from their interaction. The main contributions of the work are: (i) a global path planning that minimizes the expected length in a stochastic map; (ii) a cooperative sensor-based local re-planning in the stochastic map; (iii) a multi-vehicle consistent localization (robot pose tracking) in stochastic maps built from the multiple views of the robots; (iv) a hybrid centralized–distributed architecture, using real-time wireless communication. The robot leader centralizes the information and executes the global processes, whilst navigation of the follower robots is distributed and executed by them. Experimental results, both in simulation and in real office-like settings, illustrate the performance of each of the techniques developed and of the whole system.

The rest of this paper is organized as follows: Section 2 presents robot formations from the perspective of the leader-following approach, where a dynamical model based on the virtual spring–damper analogy is used; Section 3 introduces the concepts of traversability maps and minimum expected length paths in a generic graph-based framework; in Section 4 this concept is applied to obtain global minimum expected length paths from a global traversability map derived from a featured-based stochastic map; Section 5 describes pose tracking and cooperative local path planning and navigation for the robot formation in dynamic scenarios; Section 6 reports both simulated and real experiments obtained by a robot formation in the presence of sensing, localization and mapping uncertainties as described throughout the paper; finally, Section 7 elaborates on the conclusions of the reported work and suggests continuation lines for the near future.

Section snippets

Probabilistic robot formations

We consider formations made up of r+1 robots where the leader R0 is virtually linked to each of the followers R1,,Rr. Each link is composed by two spring–damper systems: one is in charge of maintaining the distance among the robots and the other controls the relative angle. These links induce virtual forces on the robots, producing their movements, as shown in Fig. 1.

Let the location of the formation be represented by a discrete-time stochastic state vector xR=[xR0B,xR1R0,,xRjR0,,xRrR0]T

Path planning in traversability maps

Path planning for robot formations can be stated globally for all the robots of the team. But that approach has a high computational burden. In this work, we propose a flexible alternative approach, computationally lighter. The path planning is achieved by the leader of the formation, and the followers maintain the formation adapting to the environment by means of a spring–damperanalogy (see more details in [3]). The formation is maintained while possible, otherwise it adapts its shape to the

Global traversability map

To define the global traversability map we need a set of vertices and the corresponding edges among them with the corresponding length and traversability probability values. Let a stochastic feature-based representation (e.g., built by a SLAM algorithm), maybe partial and incomplete, either of the navigation area or of the local cooperative perception of the members of the robot formation, be represented by a set of geometric features yF={yF1,yF2,,yFN} known wrt a certain reference frame (not

Cooperative replanning

In wide-opened and uncluttered environments once a global minimum expected length path for the robot leader is obtained, the robot followers would execute their paths, from origin v0 to destination vn, thanks to the links derived from the virtual spring–damper analogy of the robot formation and its commanded geometrical shape. However, in real settings frequent real-time replanning is mandatory to avoid unmapped or dynamic objects in the environment while flexibly and adaptively maintaining the

Experimental results

In this section we report both simulated and real experiments obtained by a formation in the presence of sensing, localization and mapping uncertainties as described along the previous sections. We first illustrate, from a simulation-based perspective (Player–Stage simulation environment): (i) the computation of global and local optimal plans; (ii) the adaptability of the geometric structure of the robot formation to the dynamics of the environment; and (iii) the benefits of cooperative

Conclusions

This paper has reported an tightly integrated real-time system for robot formations in the presence of sensing, localization and mapping uncertainties. A expected length of global paths for the robot leader is minimized, which considers the probability of choosing a way in which the robots have to come back in the event of finding non traversable zones whilst navigating, leading to longer paths. The selected way is tracked by the formation thanks to a cooperative sensor-based replanning

Acknowledgments

This work was partially supported by the Spanish project DGA-FSE (T04) and the MINECO-FEDERprojects DPI2012-36070, DPI2015-68905-P and DPI2012-32100.

Pablo Urcola is a researcher in the Robotics, Perception and Real Time group at the University of Zaragoza, Spain. He received his M.S. and his Ph.D. degrees in Computer Engineering from the University of Zaragoza in 2008 and 2015, respectively. His research interest include autonomous navigation, motion planning and cooperative robotics.

References (36)

  • GómezJ.V. et al.

    Planning robot formations with fast marching square including uncertainty conditions

    Robot. Auton. Syst.

    (2013)
  • CastellanosJ.A. et al.

    Robocentric map joining: Improving the consistency of EKF-SLAM

    Robot. Auton. Syst.

    (2007)
  • PapadimitriouC. et al.

    Shortest paths without a map

    Theoret. Comput. Sci.

    (1991)
  • BeardR.W. et al.

    A coordination architecture for spacecraft formation control

    IEEE Trans. Control Syst. Technol.

    (2001)
  • MacArthur, E.Z. and Crane, C. D. Compliant formation control of a multi-vehicle system, in: Proc. IEEE International...
  • UrcolaP. et al.

    Cooperative navigation using environment compliant robot formations

  • LewisM.A. et al.

    High precision formation control of mobile robots using virtual structures

    Auton. Robots

    (1997)
  • KurazumeR. et al.

    Cooperative positioning with multiple robots

  • RekleitisI.M. et al.

    Multi-robot exploration of an unknown environment, efficiently reducing the odometry error

  • SandersonA.C.

    A distributed algorithm for cooperative navigation among multiple mobile robots

    Adv. Robot.

    (1998)
  • BurgardW. et al.

    Estimating the absolute position of a mobile robot using position probability grids

  • LázaroM.T. et al.

    Localization of probabilistic robot formations in SLAM

  • FoxD. et al.

    A probabilistic approach to collaborative multi-robot localization

    Auton. Robots

    (2000)
  • RoumeliotisS.I. et al.

    Distributed multi-robot localization

    IEEE Trans. Robot. Autom.

    (2002)
  • MartinelliA. et al.

    Multi-robot localization using relative observations

  • D. Ferguson, M. Likhachev, A. Stentz, A guide to heuristic-based path planning, in: Proc. of the International Workshop...
  • KurniawatiH. et al.

    Global motion planning under uncertain motion, sensing, and environment map

    Auton. Robots

    (2012)
  • BergJ.V.D. et al.

    LQG-MP: Optimized path planning for robots with motion uncertainty and imperfect state information

    Int. J. Robot. Res.

    (2011)
  • Cited by (14)

    • Swarm robots in mechanized agricultural operations: A review about challenges for research

      2022, Computers and Electronics in Agriculture
      Citation Excerpt :

      Ha et al. (2019) present planning and control of the movement of robots in environments with uncertainties through the development of the topology control method guided by the integral path. Urcola et al. (2017) present a new technique for planning robot formations using stochastic maps; this technique computes the most likely global path in defining the expected minimum length. Agricultural swarm robotics needs the definition of the algorithms that can be used because of the processing capacity of the machines about the extraordinarily complex and multiple decision-making problems required in unstructured agricultural environments and which has objects very fragile and variable (living beings).

    • Chance constrained reachability in environments with spatially varying energy costs

      2019, Robotics and Autonomous Systems
      Citation Excerpt :

      For deterministic edge costs, standard shortest path algorithms such as Dijkstra’s algorithm [7] may be used. Planning under uncertainty is more challenging, with interesting cases such as finding the shortest expected path when there is a probability that edges are not traversable [32,33]. We consider the case in which edges are known to be traversable, however, the edge costs are uncertain.

    • Simultaneous versus joint computing: A case study of multi-vehicle parking motion planning

      2017, Journal of Computational Science
      Citation Excerpt :

      Typically, sample-based path planners are adopted to generate the “given paths” because they run fast (e.g., [1,12,13]). The third (partition-based) branch is about dividing the centralized MVMP formulation into multiple parts, stages, or levels, e.g., task assignment + motion planning [14], task assignment + motion coordination [15], and subgrouping + motion planning [16,17]. The priorities in the first branch, if not appropriately assigned, weaken the cooperative capability of a vehicle team.

    • WiFresh: Age-of-Information from Theory to Implementation

      2021, Proceedings - International Conference on Computer Communications and Networks, ICCCN
    View all citing articles on Scopus

    Pablo Urcola is a researcher in the Robotics, Perception and Real Time group at the University of Zaragoza, Spain. He received his M.S. and his Ph.D. degrees in Computer Engineering from the University of Zaragoza in 2008 and 2015, respectively. His research interest include autonomous navigation, motion planning and cooperative robotics.

    María T. Lázaro received the M.S. and Ph.D. degrees in Systems Engineering and Computer Science from the University of Zaragoza, Spain in 2009 and 2015, respectively. Currently, she is a Post Doctoral researcher with the Dipartimento di Ingegneria Informatica Automatica e Gestionale Antonio Ruberti, Sapienza University of Rome. Her current research interests include robot formations, multi-robot simultaneous localization and mapping and multi-robot human–robot interaction.

    José A. Castellanos received the M.S. and Ph.D. degrees in Industrial Electrical Engineering from the University of Zaragoza, Spain, in 1994 and 1998, respectively. Currently, he is Dean of Escuela de Ingeniera y Arquitectura, University of Zaragoza and an Associate Professor with the Departamento de Informtica e Ingeniera de Sistemas, University of Zaragoza, where he is in charge of courses in SLAM, Control Engineering and DEVS and Continuous Simulation. His current research interests include multisensor fusion and integration, Bayesian estimation in nonlinear systems and simultaneous localization and mapping.

    Luis Montano was born on September 6, 1958 in Huesca, Spain. He received the Industrial Engineering degree in 1981 and the Ph.D. degree in 1987 from the University of Zaragoza, Spain. He is a Full Professor of Systems Engineering and Automatic Control at the University of Zaragoza (Spain). He was Head of the Computer Science and Systems Engineering Department, and Associated Director and responsible of the Ambient Intelligent Division at the Aragon Institute of Engineering Research of the University of Zaragoza. He is the coordinator of the Robotics, Perception and Real Time group of the Institute, and he is principal researcher in national and international robotics research projects. His major research interests in robotics are motion planning and navigation, and multi-robot systems, more specically task planning and allocation and planning in dynamic environments with communication restrictions.

    View full text