No Belief Propagation Required
Total Page:16
File Type:pdf, Size:1020Kb
Article The International Journal of Robotics Research No belief propagation required: Belief 1–43 © The Author(s) 2017 Reprints and permissions: space planning in high-dimensional state sagepub.co.uk/journalsPermissions.nav DOI: 10.1177/0278364917721629 spaces via factor graphs, the matrix journals.sagepub.com/home/ijr determinant lemma, and re-use of calculation Dmitry Kopitkov1 and Vadim Indelman2 Abstract We develop a computationally efficient approach for evaluating the information-theoretic term within belief space plan- ning (BSP), where during belief propagation the state vector can be constant or augmented. We consider both unfocused and focused problem settings, whereas uncertainty reduction of the entire system or only of chosen variables is of interest, respectively. State-of-the-art approaches typically propagate the belief state, for each candidate action, through calcula- tion of the posterior information (or covariance) matrix and subsequently compute its determinant (required for entropy). In contrast, our approach reduces runtime complexity by avoiding these calculations. We formulate the problem in terms of factor graphs and show that belief propagation is not needed, requiring instead a one-time calculation that depends on (the increasing with time) state dimensionality, and per-candidate calculations that are independent of the latter. To that end, we develop an augmented version of the matrix determinant lemma, and show that computations can be re-used when eval- uating impact of different candidate actions. These two key ingredients and the factor graph representation of the problem result in a computationally efficient (augmented) BSP approach that accounts for different sources of uncertainty and can be used with various sensing modalities. We examine the unfocused and focused instances of our approach, and compare it with the state of the art, in simulation and using real-world data, considering problems such as autonomous navigation in unknown environments, measurement selection and sensor deployment. We show that our approach significantly reduces running time without any compromise in performance. Keywords Belief space planning, active SLAM, informative planning, active inference, autonomous navigation 1. Introduction observable Markov decision process (POMDP), while cal- culating an optimal solution of a POMDP was proven to be Decision making under uncertainty and belief space plan- computationally intractable (Kaelbling et al., 1998) for all ning (BSP) are fundamental problems in robotics and arti- but the smallest problems due to curse of history and curse ficial intelligence, with applications including autonomous of dimensionality. Recent research has therefore focused on driving, surveillance, sensor deployment, object manipu- the development of sub-optimal approaches that trade-off lation, and active simultaneous localization and mapping optimality and runtime complexity. These approaches can (SLAM). The goal is to autonomously determine the best be classified into those that discretize the action, state, and actions according to a specified objective function, given measurement spaces, and those that operate over continuous the current belief about random variables of interest that spaces. could represent, for example, robot poses, a tracked tar- get, or mapped environment, while accounting for different sources of uncertainty. 1Technion Autonomous Systems Program (TASP), Technion - Israel Insti- Since the true state of interest is typically unknown and tute of Technology, Haifa, Israel only partially observable through acquired measurements, 2Department of Aerospace Engineering, Technion - Israel Institute of it can only be represented through a probability distribution Technology, Haifa, Israel conditioned on available data. BSP and decision-making approaches reason how this distribution (the belief ) evolves Corresponding author: Dmitry Kopitkov, Technion Autonomous Systems Program (TASP), Tech- as a result of candidate actions and future expected obser- nion - Israel Institute of Technology, Haifa 32000, Israel. vations. Such a problem is an instantiation of a partially Email: [email protected] 2 The International Journal of Robotics Research 00(0) Approaches from the former class include point-based matrices for each candidate action, and do so from scratch value iteration methods (Pineau et al., 2006), simula- (Zimmerman, 2006; Zhu and Stein, 2006). tionbased (Stachniss et al., 2005), and sampling-based A similar situation also arises in measurement selection approaches (Agha-Mohammadi et al., 2014; Prentice and (Carlone et al., 2014; Davison, 2005) and graph pruning Roy, 2009). On the other hand, approaches that avoid dis- (Carlevaris-Bianco et al., 2014; Huang et al., 2012; Mazu- cretization are often termed direct trajectory optimization ran et al., 2014; Vial et al., 2011) in the context of long-term methods (e.g. Indelman et al., 2015; Patil et al., 2014; autonomy in SLAM. In the former case, the main idea is to Platt et al., 2010; Van Den Berg et al., 2012; Walls et determine the most informative measurements (e.g. image al., 2015); these approaches typically calculate a locally features) given measurements provided by robot sensors, optimal solution from a given nominal solution. thereby discarding uninformative and redundant informa- Decision making under uncertainty, also sometimes tion. Such a process typically involves reasoning about MI referred to as active inference, and BSP can be for- (see e.g. Chli and Davison, 2009; Davison, 2005), for each mulated as selecting an optimal action from a set of candidate selection. Similarly, graph pruning and sparsifi- candidates, based on some cost function. In information- cation can be considered as instances of decision making based decision making, the cost function typically con- in high-dimensional state spaces (Carlevaris-Bianco et al., tains terms that evaluate the expected posterior uncertainty 2014; Huang et al., 2012), with decision corresponding to upon action execution, with commonly used costs includ- determining what nodes to marginalize out (Ila et al., 2010; ing (conditional) entropy and mutual information (MI). Kretzschmar and Stachniss, 2012), and avoiding the result- Thus, for Gaussian distributions the corresponding cal- ing fill-in in an information matrix by resorting to sparse culations typically involve calculating a determinant of a approximations of the latter (Carlevaris-Bianco et al., 2014; posteriori covariance (information) matrices and, more- Huang et al., 2012; Mazuran et al., 2014; Vial et al., 2011). over, these calculations are to be performed for each Also here, existing approaches typically involve calcula- candidate action. tion of the determinant of large matrices for each candidate Decision making and BSP become even more chal- action. lenging problems when considering high-dimensional state In this paper we develop a computationally efficient spaces. Such a setup is common in robotics, for example and exact approach for decision making and BSP in high- in the context of BSP in uncertain environments, active dimensional state spaces that addresses the aforementioned SLAM, sensor deployment, graph reduction, and graph challenges. The key idea is to use the (augmented) general sparsification. In particular, calculating a determinant of matrix determinant lemma to calculate action impact with information (covariance) matrix for an n-dimensional state complexity independent of state dimensionality n,whilere- is in general O(n3), and is smaller for sparse matrices as in using calculations between evaluating impact for different SLAM problems (Bai et al., 1996). candidate actions. Our approach supports general observa- Moreover, state-of-the-art approaches typically perform tion and motion models, and non-myopic planning, and is these calculations from scratch for each candidate action. thus applicable to a wide range of applications such as those For example, in the context of active SLAM, state-of- mentioned above, where fast decision making and BSP in the-art BSP approaches first calculate the posterior belief high-dimensional state spaces is required. within the planning horizon, and then use that belief to Although many particular domains can be specified as evaluate the objective function, which typically includes an decision-making and BSP problems, they all can be clas- information-theoretic term (Huang et al., 2005; Indelman sified into two main categories, one where state vector is et al., 2015; Kim and Eustice, 2014; Valencia et al., 2013). fixed during belief propagation and another where the state These approaches then determine the best action by per- vector is augmented with new variables. Sensor deployment forming the mentioned calculations for each action from is an example of the first case, while active SLAM, where a given set of candidate actions, or by local search using future robot poses are introduced into the state, is an exam- dynamic programming or gradient descent (for continuous ple of the second case. Conceptually the first category is a setting). particular case of the second, but as we will see both will Sensor deployment is another example of decision mak- require different solutions. Therefore, in order to differenti- ing in high-dimensional state spaces. The basic formula- ate between these two categories, in this paper we consider tion of the problem is to determine locations to deploy the first category (fixed-state) as a BSP problem, and the the sensors such that