Feedback-based Information Roadmap
some text

The objective of our research is to present feedback-based information roadmap (FIRM), a multi-query approach for planning under uncertainty which is a belief-space variant of probabilistic roadmap methods. The crucial feature of FIRM is that the costs associated with the edges are independent of each other, and in this sense it is the first method that generates a graph in belief space that preserves the optimal substructure property. From a practical point of view, FIRM is a robust and reliable planning framework. It is robust since the solution is a feedback and there is no need for expensive replanning. It is reliable because accurate collision probabilities can be computed along the edges. In addition, FIRM is a scalable framework, where the complexity of planning with FIRM is a constant multiplier of the complexity of planning with PRM. FIRM is developed as an abstract framework. As a concrete instantiation of FIRM, we can adopt different belief stabilizers. For example, utilising stationary linear quadratic Gaussian (SLQG) controllers as belief stabilizers, we introduce the so-called SLQG-FIRM.



SLQG-FIRM


Stationary Linear Quadratic Gaussian (SLQG) Feedback-based Information Roadmap is a instantiation of FIRM composed of a Kalman filter as the state estimator and a linear quadratic regulator controller. The Linear Quadratic Gaussian controller drives beliefs to a stationary point; that is, it acts as the belief stabilizer.

The properties of Linear Quadratic Gaussian controllers allow for provably being able to drive distribution around a point to the point with zero velocity. It also guarantees the existence and construction of these stationary distributions. In SLQG-FIRM, the nodes are stationary beliefs which is characterized by the mean and a positive-semidefinite matrix. Notably, the mean of the belief is the original node from the probabilistic roadmap (PRM). The edges are local controllers which drive the system from one node to another node by following the PRM edge with a LQG controller. The SLQG-FIRM, at a high-level, constructs the roadmap in the following way:

  • Take a probabilistic roadmap (PRM) with nodes and edges which are already validity checked.
  • For each of these nodes, design the node controller for which the node is the mean of the belief
  • For each edge, design the edge controller (linear qudaratic gaussian controller) to follow the edge and construct the respective local controller.
This method works for holonomic linear systems with quadratic costs and gaussian noise (LQG).

 

DFL-FIRM


Dynamic Feedback Linearization-based (DFL) FIRM is another concrete instantiation of FIRM for nonholonomic systems which struggle to achieve belief reachabilty. DFL-FIRM uses DFL-based controllers as belief controllers which allows for the system to stabilize to belief nodes. Utilizing these DFL-based controllers, DFL-FIRM still preserves the "principle of optimality" and all other FIRM properties.

Robust Online Belief Space Planning


Robust Online Belief Space Planning in Changing Environments: Application to Physical Mobile Robots

This section describes how to address the physical issues that arise when trying to conduct motion planning under uncertainty with FIRM.

Method overview

Proposed
Robust motion planning under uncertainty with FIRM is done with a dynamic replanning scheme. FIRM replans using receding horizon control in the belief space until the goal is found.
  • FIRM solves an optimization problem to find the optimal policy from a given node to the goal node.
  • Firm finds and applies the control which corresponds to the optimal policy at its given belief state<\li>
  • Observe the new observation
  • Compute the new belief
Notably, the optimal policy is recomputed at each iteration. Lazy Feedback Evaluation in Changing Environments To address changing environments, an online planner can use lazy evaluation. The basic idea is that at every node the robot re-evaluates only the next edge that it needs to take or a limited set of edges in the vicinity of the robot. By re-evaluation, we mean re-compute the collision probabilities along the edges. If needed, the dynamic programming problem is re-solved and a new feedback tree is computed. Handling the kidnapped robot problem To address this problem, a robot must be able to detect a kidnapped situation, localize itself, and replan. These issues can be handled respectively as follows:
  • A robot running firm can have an internal record of the relative ranges and bearings of the robot to landsmarks, noticing it has been kidnapped if a threshold is met
  • The robot enters an information gathering mode with a large covariance matrix
  • The robot need only traverse to the nearest node, since the FIRM graph is a multi-query graph

SLAP


SLAP: Simultaneous Localization and Planning Under Uncertainty for Physical Mobile Robots via Dynamic Replanning in Belief Space