Safe and Distributed Motion Planning and Replanning

Greedy but Safe Replanning

It becomes increasingly important to reason about the dynamics of robots during planning so as to achieve a higher degree of autonomy by producing plans that can be safely and directly followed. Examples of motion constraints that have to be respected are bounds in velocity and acceleration that cause drift. At the same time, in many realistic tasks, robots have only partial information about their environment. Examples incude planning among unknown static obstacles, exploration and planning in dynamic environments. Solving such problems requires an interleaving of sensing, planning and execution. This means that a planner is called frequently and has finite time to replan a trajectory.

This project focuses on the safety concerns that arise when replanning for systems that exhibit non-trivial kinodynamic constraints. Safety becomes an issue because a collision-free trajectory may still bring a dynamical system close to an obstacle
region with high-velocity and no possible maneuver to
avoid collisions into the future as the figure above illustrates. Similar problems also arise when planning in dynamic environments, even for systems without challenging dynamics, because the obstacles may block escaping maneuvers.

Over the last decade, sampling-based planners that construct a tree data structure of feasible trajectories, such as RRT and its variants, have become popular for planning with dynamic constraints. Thus, it is important to show how ICS avoidance can be achieved effectively in the context of tasks that require replanning using sampling-based algorithms, such as the exploration challenge illustrated above. Nevertheless, identifying whether a state is ICS or not is computationally challenging for most realistic problems. For moving obstacles, it also requires knowledge of the obstacles’ paths.

Some methods for ICS avoidance employ machine learning offline or approximation methods in order to construct an ICS identifier that can be used online to prune those states identified as ICS. These methods have the advantage of being fast in their ICS determination during the online step but they can misclassify states. Thus, these methods do not provide safety guarantees even for simpler replanning tasks, such as planning among unknown static obstacles.

This work develops conservative alternative, which identify a state as safe by explicitly computing the ICS set and reasoning over an infinite time horizon. These methods provide stronger safety guarantees, which is highly desirable, but they also require more online computations, which can also effect the practical safety of the approach. A slow planner delays taking into account new sensing information, while a faster planner provides a more diverse set of plans in the same amount of time.

The speed of a replanning scheme that aims to avoid
ICS does not only depend on the computational cost of the method for identifying ICS. It also depends on how many calls to an ICS-identification method are made by the high-level replanning approach. Our work has produced conservative methods that guarantee safety in many important applications, while minimizing the computational cost of providing these guarantees. The following figure provides an illustration of the increase in the number of alternative paths that can be constructed over the same period of time if our safety approach is employed.

The following videos illustrate replanning tasks that have been achieved while employing the safety algorithms developed by this project:

Distributed case

Autonomous vehicles have long been the focus of robotics research. The progress in wireless networking allows to consider groups of vehicles that operate in the same environment and use communication to coordinate their motion. Moreover, it gives rise to the idea of networks of vehicles that jointly solve a task while retaining connectivity. Using such teams of multiple, coordinating vehicles offers redundancy and robustness in the execution of many tasks (e.g. space exploration, autonomous demining). Nevertheless, the control of such systems involves multiple research challenges.

In this work, the focus is on motion planning and coordination challenges. So far it has been shown, that given procedures for updating a vehicle’s map, state and goal, it is possible to design feasible, collision-free trajectories for multiple vehicles operating in the same, partially-known environment in a synchronized manner. In particular, we deal with the safety concerns that arise due to the kinodynamic motion constraints (e.g., bounded velocity and acceleration, smooth steering) that real vehicles exhibit. Thus, we are studying how inter-vehicle communication can be utilized in this context to achieve safe motion coordination.

We are interested in a solution with the following characteristics:

  • A general and abstract algorithm that is not limited to specific system dynamics or to specific types of workspaces and obstacles.
  • A scalable, distributed solution that respects physical limitations in sensing and communication and avoids centralized computation.
  • A real-time algorithm, as typically vehicles do not have global knowledge of their workspace. This means that sensing, planning and execution are interleaved and there is limited amount of time to compute a partial plan towards the goal.
  • A safe solution for systems with second-order constraints. The algorithm must provide guarantees for collision-avoidance and the retainment of a communication network if desired by the team.

The approach involves the integration of sampling-based kinodynamic planners with message-passing protocols to distributedly control the motion of multiple communicating vehicles in simulation. It is an extension of work on single-agent, safe, real-time sampling-based planning to the case of multiple networked vehicles with limited communication range. It provides theoretical safety guarantees in terms of avoiding inevitable collision or loss of connectivity states. Compared to alternative approaches for decentralized planning it can be applied on different environments and systems with various dynamics. In contrast to existing work on planning for dynamic networks, where coordination is centralized, the approach aims to distribute computation.

The starting point for the method is to identify the sufficient information exchanged between the vehicles so as to plan safe trajectories. These sufficient conditions dictate the proposed algorithms. Each vehicle uses a sampling-based planner to generate feasible trajectories that allow the existence of safe alternatives to other vehicles. For coordinating the selection of compatible trajectories between vehicles, we first tested a priority-based scheme. This allows us to provide a proof that the vehicles always have safe trajectories to follow. The priority scheme is then replaced by an asynchronous, message-passing protocol, which provides the same theoretical safety guarantees. Among the safe solutions and given the available time, the asynchronous protocol optimizes a joint payoff function.

The proposed method has been implemented on a multiprocessor simulator. Each processor models a vehicle and communicates asynchronously with other processors. The experimental results confirm the theoretical guarantees of collision avoidance and network retainment for second-order vehicles jointly exploring an unknown workspace. The distributed protocol has computational and scaling advantages when compared against the prioritized scheme.

The following figure shows 15 vehicles exploring a maze-like environment, while retaining a vehicular network.

The following videos illustrate exploration tasks executed by a vehicular network, while employing the safety algorithms developed by this project:

Asynchronous Extension

Related Publications


This work has been supported by NSF CNS 0932423. Any opinions and findings expressed in this paper are those of the authors and do not necessarily reflect the views of the sponsor.

People Involved