Dynamic Feedback Linearization-based Belief Stabilization for Nonholonomic Motion Planning in Belief Space

In roadmap-based methods, such as the Probabilistic Roadmap Method (PRM) in deterministic environments or the Feedbackbased Information RoadMap (FIRM) in partially observable probabilistic environments, a stabilizing controller is needed to guarantee node reachability in state or belief space. In belief space, it has been shown that the belief-node reachability can be achieved using stationary Linear Quadratic Gaussian (LQG) controllers, for linearly controllable systems. However, for nonholonomic systems such as unicycle model, belief reachability is a challenge. In this paper we construct a roadmap in information space, where the local planners in partially-observable space are constructed by utilizing a Kalman filter as an estimator along with a Dynamic Feedback Linearization-based (DFL-based) controller as the belief controller. As a consequence the task of belief stabilizing to the pre-defined nodes in the belief space is accomplished even for nonholonomic systems. Therefore, a queryindependent roadmap is generated in belief space that preserves the “principle of optimality”, required in dynamic programming solvers. while taking obstacles into account, this method serves as an offline POMDP solver for motion planning in belief space. Experimental results shows the efficiency of both individual local planners and the overall planner over information graph for a nonholonomic model. Agha-mohammadi and Amato are with the Dept. of Computer Science and Engineering and Chakravorty is with the Dept. of Aerospace, Texas A&M University, TX 77843, USA. Emails: [email protected], [email protected], and [email protected]