IROS2019国际学术会议论文集 1870

Learning State Dependent Sensor Measurement Models for Localization Troi Williams and Yu Sun Abstract A 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 robot s 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 robot s camera Observing noisy measurements in turn can trans late into noisy estimates of the robot s 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 yusun mail usf edu 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 robot s 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 robot s 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 robot s 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 network s output layer and the network s 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 i i 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 mobile robot because the EKF is a popular state estimator and land based mobile robots are ubiquitous However the underlying principles are not limited to this specifi c example the EKF or land based mobile robots Other robots such as quadcopters fi lters such as the Particle Filter and environments can use the underlying principles A Extended Kalman Filter Before we describe the Extended Kalman Filter for our land based mobile robot we introduce the following no tations Let xk xk yk k Tdenote the global pose 2D position and orientation of a robot and let ukdenote an odometry command Let zkdenote a sensor measurement that observed a landmark which has a global 2D position mx y Let the subscript k denote an EKF time step during runtime Finally we use the hat notation to denote predicted values such as states An Extended Kalman Filter is gov