Linearization in Motion Planning under Uncertainty

Motion planning under uncertainty is essential to autonomous robots. Over the past decade, the scalability of such planners have advanced substantially. Despite these advances, the problem remains difficult for systems with non-linear dynamics. Most successful methods for planning perform forward search that relies heavily on a large number of simulation runs. Each simulation run generally requires more costly integration for systems with non-linear dynamics. Therefore, for such problems, the entire planning process remains relatively slow. Not surprisingly, linearization-based methods for planning under uncertainty have been proposed. However, it is not clear how linearization affects the quality of the generated motion strategy, and more importantly where to and where not to use such a simplification. This paper presents our preliminary work towards answering such questions. In particular, we propose a measure, called Statistical-distance-based Non-linearity Measure (SNM), to identify where linearization can and where it should not be performed. The measure is based on the distance between the distributions that represent the original motion-sensing models and their linearized version. We show that when the planning problem is framed as the Partially Observable Markov Decision Process (POMDP), the difference between the value of the optimal strategy generated if we plan using the original model and if we plan using the linearized model, can be upper bounded by a function linear in SNM. We test the applicability of this measure in simulation via two venues. First, we compare SNM with a negentropy-based Measure of Non-Gaussianity (MoNG) —a measure that has recently been shown to be a suitable measure of non-linearity for stochastic systems [1]. We compare their performance in measuring the difference between a general POMDP solver [2] that computes motion strategies using the original model and a solver that uses the linearized model (adapted from [3]) on various scenarios. Our results indicate that SNM is more suitable in taking into account the effect that obstacles have on the effectiveness of linearization. In the second set of tests, we use a local estimate of SNM to develop a simple on-line planner that switches between using the original and the linearized model. Simulation results on a car-like robot with second order dynamics and a 4-DOFs and 6-DOFs manipulator with torque control indicate that our simple planner appropriately decides if and when linearization should be use