Simultaneous localization and Planning (SLAP) is a crucial ability for an autonomous robot operating under uncertainty. In its most general form, SLAP induces a continuous POMDP (partially-observable Markov decision process), which needs to be repeatedly solved online. This paper addresses this problem and proposes a dynamic replanning scheme in belief space. The underlying POMDP, which is continuous in state, action, and observation space, is approximated offline via sampling-based methods, but operates in a replanning loop online to admit local improvements to the coarse offline policy. This construct enables the proposed method to combat changing environments and large localization errors, even when the change alters the homotopy class of the optimal trajectory. It further outperforms the state-of-the-art FIRM (Feedback-based Information RoadMap) method by eliminating unnecessary stabilization steps. Applying belief space planning to physical systems brings with it a plethora of challenges. A key focus of this paper is to implement the proposed planner on a physical robot and show the SLAP solution performance under uncertainty, in changing environments and in the presence of large disturbances, such as a kidnapped robot situation.
Continuous Gaussian POMDP solvers A different class of methods restrict the form of uncertainty to Gaussian and extend the traditional deterministic motion planning methods (e.g., PRM and RRT) to belief space. Examples include @cite_13 @cite_10 , @cite_8 . More recently, @cite_26 proposes an efficient approximate value iteration method. Starting from an initial solution (trajectory in belief space), it converges to the closest local minimum to the initial solution. These methods are single query (the solution is valid for a given initial belief), thus in case of replanning from a new belief most of the computation needs to be redone. Replanning becomes more challenging when the planner has to switch the plan from one homotopy class to another.
@cite_10 @cite_26 @cite_13 @cite_8
2002201291 2399169217 2289207600 2103052138
In this paper we present LQG-MP (linear-quadratic Gaussian motion planning), a new approach to robot motion planning that takes into account the sensors and the controller that will be used during the execution of the robotâs path. LQG-MP is based on the linear-quadratic controller with Gaussian models of uncertainty, and explicitly characterizes in advance (i.e. before execution) the a priori probability distributions of the state of the robot along its path. These distributions can be used to assess the quality of the path, for instance by computing the probability of avoiding collisions. Many methods can be used to generate the required ensemble of candidate paths from which the best path is selected; in this paper we report results using rapidly exploring random trees (RRT). We study the performance of LQG-MP with simulation experiments in three scenarios: (A) a kinodynamic car-like robot, (B) multi-robot planning with differential-drive robots, and (C) a 6-DOF serial manipulator. We also present a method that applies Kalman smoothing to make paths Ck-continuous and apply LQG-MP to precomputed roadmaps using a variant of Dijkstraâs algorithm to efficiently find high-quality paths. We introduce a highly efficient method for solving continuous partially-observable Markov decision processes (POMDPs) in which beliefs can be modeled using Gaussian distributions over the state space. Our method enables fast solutions to sequential decision making under uncertainty for a variety of problems involving noisy or incomplete observations and stochastic actions. We present an efficient approach to compute locally-valid approximations to the value function over continuous spaces in time polynomial (O[n4]) in the dimension n of the state space. To directly tackle the intractability of solving general POMDPs, we leverage the assumption that beliefs are Gaussian distributions over the state space, approximate the belief update using an extended Kalman filter (EKF), and represent the value function by a function that is quadratic in the mean and linear in the variance of the belief. Our approach iterates towards a linear control policy over the state space that is locally-optimal with respect to a user defined cost function, and is approximately valid in the vicinity of a nominal trajectory through belief space. We demonstrate the scalability and potential of our approach on problems inspired by robot navigation under uncertainty for state spaces of up to 128 dimensions. Many methods for planning under uncertainty operate in the belief space, i.e., the set of probability distributions over states. Although the problem is computationally hard, recent advances have shown that belief-space planning is becoming practical for many medium size problems. Some of the most successful methods utilize sampling and often rely on distances between beliefs to partially guide the search process. This paper deals with the question of what is a suitable distance function for belief space planning, which despite its importance remains unanswered. This work indicates that the rarely used Wasserstein distance (also known as Earth Mover’s Distance ( ( EMD ))) is a more suitable metric than the commonly used ( L1 ) and Kullback–Leibler ( ( KL )) for belief-space planning. Simulation results on Non-Observable Markov Decision Problems, i.e., the simplest class of belief-space planning, indicate that as the problem becomes more complex, the differences on the effectiveness of different distance functions become quite prominent. In fact, in state spaces with more than 4 dimensions, by just replacing ( L1 ) or ( KL ) distance with ( EMD ), the problems become from virtually unsolvable to solvable within a reasonable time frame. Furthermore, preliminary results on Partially Observable Markov Decision Processes indicate that point-based solvers with ( EMD ) use a smaller number of samples to generate policies with similar qualities, compared to those with ( L1 ) and ( KL ). This paper also shows that ( EMD ) carries the Lipschitz continuity of the state’s cost function to Lipschitz continuity of the expected cost of the beliefs. Such a continuity property is often critical for convergence to optimal solutions. In this paper we address the problem of motion planning in the presence of state uncertainty, also known as planning in belief space. The work is motivated by planning domains involving nontrivial dynamics, spatially varying measurement properties, and obstacle constraints. To make the problem tractable, we restrict the motion plan to a nominal trajectory stabilized with a linear estimator and controller. This allows us to predict distributions over future states given a candidate nominal trajectory. Using these distributions to ensure a bounded probability of collision, the algorithm incrementally constructs a graph of trajectories through state space, while efficiently searching over candidate paths through the graph at each iteration. This process results in a search tree in belief space that provably converges to the optimal path. We analyze the algorithm theoretically and also provide simulation results demonstrating its utility for balancing information gathering to reduce uncertainty and finding low cost paths.