IROS2019国际学术会议论文集1870

Learning State-Dependent, Sensor Measurement Models for Localization Troi Williams and Yu Sun AbstractA robot typically relies on sensor measurements to infer its state and the state of its environment. Unfortunately, sensor measurements are noisy, and the amount of noise can vary with state. The literature provides a collection of methods that estimate and adapt measurement noise over time. However, many methods do not assume that measurement noise is stochastic, or they do not estimate sensor measurement bias and noise based on state. In this paper, we propose a novel method called state-dependent, sensor measurement models (SDSMMs). This method: 1) learns to estimate measurement probability density functions directly from sensor measurements and 2) stochastically estimates an expected measurement (which in- cludes measurement bias) and a measurement noise, both of which are conditioned upon the states of a robot and its environment. Throughout this paper, we discuss how to learn an SDSMM and use it with the Extended Kalman Filter (EKF). We then apply our method to solve an EKF localization problem using a real robot dataset. Our localization results showed that at least one of our proposed methods outperformed a standard EKF in all 15 cases for 2D position error and 10 of 15 cases for 1D orientation error. Our methods had a mean improvement of 39% for position and 15% for orientation. I. INTRODUCTION In the context of real-world mobile robots, state estimation is the problem of estimating the state of a robot 1. Typically, states such as the exact location of a robot and the exact lo- cations of obstacles in a robots environment are not directly observable. However, such states can be inferred from sensor measurements 2. Therefore, robots rely on sensors to infer their states and the states of their environment1. Although sensors play an integral role in estimating the state of a robot, their measurements are generally imperfect because they contain error (bias and noise). For example, consider a stationary, autonomous mobile robot that uses an onboard camera to measure its bearing to nearby obstacles. Each time such a robot measures its bearing to one obstacle, the robot can observe noisy bearing measurement due to several reasons (for instance, imperfections in the robots camera). Observing noisy measurements, in turn, can trans- late into noisy estimates of the robots state, and noisy state estimates can cause hazardous situations. For instance, the Troi Williams and Yu Sun are with the Department of Computer Science and Engineering, University of South Florida, Tampa, FL 33620, USA troiw, . Troi Williams has been supported by the Florida Education Funds McKnight Doctoral Fellowship Program, Alfred P. Sloan Foundation University Center of Exemplary Mentoring, and the NSF Florida-Georgia Louis Stokes Alliance for Minority Participation Bridge to the Doctorate award HRD #1400837. 1An environment generally describes a set of features that we are interested in observing or tracking (for example, the locations of landmarks). robot may incorrectly estimate that obstacles are not in its path and, as a result, may collide with those obstacles. This example illustrates that robots need an estimate of sensor measurement error so that they can determine how much they should trust a sensor measurement. However, merely knowing the average, sensor measure- ment error is not enough because the measurement error can vary depending on the states of a robot and its environment. For example, a combination of environment lighting and a camera lens imperfection can cause nearby, off-centered obstacles to appear further off to the side than they are. The environment lighting and lens imperfection, in turn, can produce bearing measurements with high bias and high noise. However, when obstacles are centered and far away, the robot may not experience the same issue as before; therefore, its measurements may have relatively less bias and less noise. This example implies that merely knowing the average sensor measurement error is not enough. Instead, a robot must be able to estimate the error of each measurement given the states of the robot and its environment. This paper introduces a novel method called state- dependent, sensor measurement models. This method learns to estimate measurement probability density functions of a sensor. Therefore, a robot can use such a model to estimate quantities such as the expected measurement (which includes measurement bias) or the expected measurement error (bias and noise) of each sensor measurement. We also demonstrate how to modify a standard, Extended Kalman Filter (EKF) to employ a state-dependent, sensor measurement model for landmark localization. We chose to focus on the EKF due to convenience and because the EKF is a widely-used state estimation algorithm. Although we introduce state- dependent, sensor measurement models with the EKF, other derivations of the Kalman Filter and other fi lters such as the Particle Filter can also use SDSMMs. II. BACKGROUND The Extended Kalman Filter (EKF) uses a measurement model to infer a robots state 3. An EKF measurement model is mainly composed of two terms: h(x,m), the ex- pected measurement (or measurement function), and Q, the covariance of the measurement noise. Generally, h(x,m) is an a priori, human-derived function that maps the states of a robot and its environment to a sensor measurement. The output of h(x,m) is z, which represents the measurement that a robot would observe at pose x and with a map m. Q, the covariance of the measurement noise, quantifi es the sensor measurement noise. Q is usually initialized using 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE3090 several methods. Some methods include: performing sensor calibration; choosing a set of values that are near zero and are adapted later; using trial and error to select a Q that yields the smallest error; and using least-squares to estimate the covariance from the residuals between the sensor measurements and the measurements without error 4. When it comes to using Q in the EKF, there are two methods for applying the covariance of the measurement noise. The fi rst method assumes Q is constant and does not change while the fi lter runs. In some applications, a constant Q works. However, generally, a constant Q can lead to fi lter divergence 5. Therefore, Q is typically adapted. The second method adapts Q as the EKF runs, and the literature provides several techniques that adapt Q in the EKF. These techniques include Bayesian, Maximum Likelihood estimation based on the innovation, Covariance Matching, and Correlation Techniques 6, 7. Typically, these methods adapt Q based on statistics computed from within the EKF. For example, covariance matching maintains a windowed, running average of the residual between a sensor measurement and an expected measurement, h(x,m). Other research has also used artifi cial intelligence and neural networks to adapt Q. 8, 9 used fuzzy logic and neural networks to estimate scaling factors that were used to adapt previously computed noise covariances as their robot moved throughout an environment. Unfortunately, these methods have at least one of several drawbacks: 1) they solely rely on internal fi lter information to estimate the measurement noise and, therefore, do not directly capture how state correlates with measurement error (bias and noise); 2) they do not stochastically estimate measurement error; or 3) they do not directly estimate measurement error. A state-dependent, sensor measurement model, our pro- posed method, has the following advantages over the previ- ous methods: a) it assumes measurement noise is stochastic and varies with the states of a robot and the robots environ- ment; b) it estimates a different measurement p.d.f. (which includes measurement bias and noise) for each state; c) it alleviates the need for precomputing measurement noise; d) it does not need to run alongside a fi lter to learn measurement noise; and e) it is fi lter agnostic. In this paper, we present the following contributions: A method that learns to estimate measurement probabil- ity density functions of a sensor using state-dependent, sensor measurement models. A discussion on using Mixture Density Networks to estimate a measurement p.d.f. A discussion on using state-dependent, sensor measure- ment models with the Extended Kalman Filter. An application of state-dependent, sensor measurement models to localization using the Extended Kalman Filter and a robot dataset. III. LEARNINGSTATE-DEPENDENT, SENSOR MEASUREMENTMODEL A. State-Dependent, Sensor Measurement Models We introduce a novel concept called state-dependent, sensor measurement models (SDSMMs). A state-dependent, sensor measurement model is a model that learns to esti- mate measurement probability density functions (p.d.f.) of a sensor, p(z|). The input to the model, , is called a combined state array. A combined state array has one or more dimensions and contains features that correlate with the measurement error (bias and noise) of a particular sensor. The features in a combined state array can include the state of a robot (which houses the sensor), the state of the robots environment, and measurements from other sensors. For example, features in combined state array can include the positions of landmarks in robot-relative coordinates, the brightness of a room, and the velocity of the robot. Also, if one chooses the elements of the combined state array appropriately, the state-dependent, sensor measurement model may generalize to unseen environments. The output of the model is a measurement p.d.f., p(z|). Each estimated measurement p.d.f. is conditioned (depends) on the input state. Since an input state and an output measure- ment p.d.f. may have different representations, an SDSMM also learns to map from a state space to the measurement space of a sensor. In addition, a state-dependent, sensor measurement model can estimate a different measurement p.d.f., p(z|), for each state . Since the output of a state-dependent, sensor measurement model is a p.d.f., one can use the p.d.f. in several ways. Some examples include sampling expected measurements, computing the probability of a measurement z occurring, and calculating a central tendency and noise of an estimated measurement p.d.f. (such as a mean and variance). One can also use the output of an SDSMM to develop competency- aware robots that determine if they can safely navigate in an environment or manipulate an object. State-dependent, sensor measurement models also have other properties. Many fi lters (such as the Particle Filter) can incorporate SDSMMs with minimal effort. A learned model and a specifi c sensor form a pair; therefore, the learned model can be used on whichever platform that sensor resides. An SDSMM can also be used in unknown environments as long as the state representation suffi ciently describes the measurement errors (bias and noise) of a sensor. Beyond state estimation, our method can also help identify states that affect measurement error. B. Learning State-Dependent, Sensor Measurement Models The model learns to estimate a sensor measurement p.d.f., p(z|), through training on sensor measurements that were observed at corresponding states. Let = 1,2,.,N denote a set where each member is a combined state array. Let Z = z1,z2,.,zN denote the set of sensor measure- ments where zqwas observed at state q. Let D denote a set that pairs a measurement, z, with its correspond- ing state, . That is, D = (z,)1,(z,)2,.,(z,)N and 3091 Fig. 1.This diagram illustrates the overall architecture of a mixture density network. This probabilistic model is capable of generating an arbitrary probability distribution p(z|) conditioned upon some input 10. its members are independently and identically distributed. f(z|;w) denotes a state-dependent, sensor measurement model with trainable parameters, w. f(z|;w) can learn to estimate measurement probability density functions of a sensor, p(z|), through learning a set of parameters, w, that maximizes the likelihood of observing the data D. Written formally, we can train our model through maximizing the likelihood function: L(w) = f(z1,z2,.,zN,1,2,.,N;w) = N Y q=1 f(zq,q;w) = N Y q=1 f(zq|q;w)f(q) (1) where L(w) is the likelihood function, f(zq|q;w) is the state-dependent, sensor measurement model that we are training, and f(q) is the prior distribution of the state set . However, in practice one usually minimizes the negative log likelihood: E = lnL. Since a state-dependent, sensor measurement model learns directly from sensor measurements and the states at which the measurements were observed, the model parameters, w, also learn how the input state correlates with measurement error (bias and noise). As a result, the trained model learns to estimate state-dependent measurement errors. In addition to measurement noise, we are also interested in learning measurement bias because a priori, human-derived mea- surement models typically do not capture how states and partial sensor calibrations affect measurement bias. As a result, these unmodelled correlations may cause a fi ltering algorithm to over- or underestimate measurement updates when correcting the predicted state of a robot. C. Mixture Density Networks A mixture density network (MDN) is a model that com- bines a feed-forward neural network with a mixture model 10. In an MDN, the mixture model serves as the networks output layer and the networks output is a parameter vec- tor y. This parameter vector contains a set of parameters that completely describe how the target data was generated 10. As a result, the mixture model layer allows an MDN to represent arbitrary probability distributions of a desired quantity, z, conditioned upon an input . Such probability distributions are defi ned by a linear combination of kernel functions and are mathematically expressed as: p(z|) = C X i=1 i()i(z|)(2) where C is the number of kernel functions in the model, i () is the i-th mixing coeffi cient, P ii() = 1, and i() is the i-th kernel function. Following 10, we use a Gaussian function as our kernel. Therefore, our kernel function, i (z|), is defi ned as: i(z|) = 1 (2)d/2 h i() idexp ? z i() ?2 2 h i() i2 (3) where d is the number of dimensions in the vector z, and i() and i() represent the conditional standard devia- tion and multivariate mean of the i-th Gaussian probability density (Fig. 1). In 10, i()2is referred to as a common variance and, unlike i(), the common variance is usually limited to one dimension. Since our MDN uses a Gaussian kernel, the parameter vector y, the output of our MDN, is comprised of a set of C conditional, Gaussian Mixture Model (GMM) parameters. Each set of GMM parameters is represented with the variables i(), i(), and i(). In addition, since these conditional variables completely describe a probability distribution p(z|), we can compute a collection of statistics about the distribution using the values in the parameter vector. 10 provides several statistics that an MDN can calculate. D. Learning State-Dependent, Sensor Measurement Models with Mixture Density Networks A state-dependent, sensor measurement model must be able to: 1) map from an input state space to a measurement space, 2) model how an input state can affect measurement error (bias and noise), and 3) estimate a sensor measurement p.d.f. In this paper, we use MDNs as state-dependent, sensor measurement models because neural networks are regarded as universal function approximators 11 and MDNs output a set of parameters that can completely describe a probability density function, both of which fulfi ll our needs. To estimate a measurement p.d.f. of a sensor, an MDN must learn a set of parameters, w, that maximizes the likelihood of observing measurements Z at states . If we have a set of measurements and their associated states, D, we can use (1) to fi nd a set of parameters, w, that maximizes the likelihood. 10 details how to train an MDN through minimizing the negative log-likelihood loss function, E. 10 3092 defi ned the negative log-likelihood loss function as E = N X q=1 ln ( C X i=1 i(q)i(zq|q) )! (4) where the pair (zq,q) is the q-th observed sensor mea- surement and input combined state array, and i(q) and i(zq|q ) represent the i-th mixture coeffi cient and Gaussian probability density function, respectively. IV. USINGSTATE-DEPENDENT, SENSORMEASUREMENT MODELS FORLOCALIZATION In this section, we fi rst describe the Extended Kalman Filter (EKF) in the context of localization. We assume the environment is a planar surface, contains a collection of landmarks, and has one robot. The robot is a land-based, mobile vehicle that navigates on the planar surface and uses the landmarks to localize (or determine its pose) in an environment. The robot also has a pre-built map that contains the position and signature (identifi er) of each landmark in the environment. Therefore, the robot only performs localization, not SLAM, in this paper. After we introduce the EKF, we describe how to use a state-dependent, sensor measurement model with the EKF. For this discussion, we assume that the reader is familiar with the EKF. Therefore, we only describe the process model, the measurement model, and the EKF equations for a land- based, mobile robot. For more details about the EKF, we provide references that detail its derivation 2, 12. In this paper, we apply state-dependent, sensor measure- ment models to an example using the EKF and a land-based, mobi