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

Abstract

This paper presents 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 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 needed 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 apply Kalman Smoothing to make paths C k -continuous while avoiding obstacles and apply LQG-MP to precomputed roadmaps using a variant of Dijkstra's algorithm to efficiently find near-optimal paths.

Safe Motion Planning for Imprecise Robotic Manipulators by Minimizing Probability of Collision

“…Our focus in this paper is on robots with uncertainty in their motion and state estimation; we do not consider uncertainty in sensing of obstacle locations (e.g., [11]) or grasping (e.g., [20]). Extensive prior work has investigated motion planning under uncertainty for mobile robots that can be approximated as points or spheres in the workspace, e.g., [26,17,18,27,2,21,19,4,8]. For point or spherical robots, computing an estimate of the probability of collision with obstacles can be done in the workspace since the geometry of the C-obstacles is low dimensional and can be directly computed.…”

“…These approaches are difficult to scale, and computational costs may prohibit their ap-plication to robots with higher dimensional configuration spaces. Another class of approaches rely on sampling-based methods to compute a path and then compute an LQG feedback controller to follow that path [26,4,21,1]. Other approaches compute a locally-optimal trajectory and an associated control policy [19,30,25].…”

“…Extensive prior work has investigated integrated motion planning and control under uncertainty for robots that can be approximated as points in the workspace (e.g., [26,25,19,4]), but robotic manipulators raise new challenges. Whereas for point robots analytically estimating probability of collisions is facilitated by the fact that the robot's configuration space and workspace share parameters, for manipulators the configuration space and workspace are disjoint.…”

Motion planning under uncertainty for on-road autonomous driving

“…However, POMDP is known to be computationally expensive and scales poorly when the problem dimension increases. LQG-based motion planning algorithms [5], [10] estimate the vehicle's states based on a Kalman filter, and capture the uncertainty during execution using a Linear-Quadratic Regulator (LQR) controller, which is then combined with a sampling-based search algorithm to find the optimal trajectory. Compared to POMDP, LQG-based methods have lower computational complexity and also scale better with the number of states.…”

“…The Kalman filter process equations for this trajectory tracking system are the same as (6) - (10).Σ − t is the covariance after the process update,Σ t is the posteriori covariance after the measurement update, andL t is the Kalman gain. According to [5], the final distribution for the state vector is:…”