Kalman Filtering for Differential Drive Robots Tracking
Total Page:16
File Type:pdf, Size:1020Kb
XIII Simp´osio Brasileiro de Automa¸c˜ao Inteligente Porto Alegre – RS, 1o – 4 de Outubro de 2017 KALMAN FILTERING FOR DIFFERENTIAL DRIVE ROBOTS TRACKING Luis Aguiar∗, Marcos R. O. A. Maximo∗, Takashi Yoneyamay, Samuel Pinto∗ ∗Autonomous Computational Systems Lab (LAB-SCA), Computer Science Division, Aeronautics Institute of Technology Pra¸ca Marechal Eduardo Gomes, 50, Vila das Ac´acias, 12228-900 S~aoJos´edos Campos, SP, Brazil yElectronic Engineering Division, Aeronautics Institute of Technology Pra¸ca Marechal Eduardo Gomes, 50, Vila das Ac´acias, 12228-900 S~aoJos´edos Campos, SP, Brazil Emails: [email protected], [email protected], [email protected], [email protected] Abstract| This paper presents the application of the stochastic filtering techniques known as Extended Kalman Filter and Uscented Kalman Filter in the tracking and state estimation of differential drive robots. This work presents and describes these algorithms, the stochastic modeling for this specific problem, the results from implementation using real robot's data and performance evaluation from simulations. Keywords| Mobile Robots, Stochastic Filtering, Tracking 1 Introduction Tracking is a particularly important task con- cerning not only the VSS challenge but many Mobile robotics is a rich and growing field in mod- other applications. Finding the motion of an ob- ern technology. Recently, it has been achieving ject can be useful in navigation, video surveil- huge technical advancements and at the same time lance, satellite control, missile guidance etc. We finding several interesting applications from mili- have experienced a great progress in the proba- tary to domestic scenarios. A key element in the bilistic approach ()(S. Thrun and Fox, 2005) and robotics field is the movement tracking and the (Brookner, 1998)) using Bayesian filtering to deal state estimation of robot agents throughout the with tracking. Many problems can be cast into environment. As can be seen in some recent so- the problem of estimating the state of a dynamic phisticated robots, these tasks play an essential system from discrete noisy measurements. There- role in the development of advanced autonomous fore, the standard formulation includes the state machines. space and discrete-time approach (Arulampalam et al., 2001), using the world famous non-linear One particular class of mobile robots is the Gaussian filters (EKF and UKF) and the non- differential drive robots, which can be widely seen parametric Particle Filter. In addition to the po- in several applications due to its hardware sim- sition estimation, in several situations, it is also plicity and versatility. In this way, an interesting necessary to extract the velocity estimates from challenge involving these robots is the IEEE Very the measurement of the position alone. For exam- Small Size competition. VSS is a soccer based ple, one can mention some feedback control mod- challenge with autonomous robots with maximum els and trajectory planning algorithms which de- dimensions of 7:5cmx7:5cmx7:5cm. Each team mand an optimal velocity estimation. Some for- has three robots in the field and is allowed to have mulations of these bayesian filters aims to solve a camera fixed above it and also develop a vision this issue, as can be seen in (Palumbo et al., 2010) system. Most of the processing occur in a exter- and in (Bruno and Pavlov, 2004). nal computer which communicates with the agents using wireless interface. Therefore, the purpose of this paper is to This work is based on our recent efforts for the present the application of the Extended Kalman development of a tracking system for the ITAn- Filter, Uscented Kalman Filter and the linear droids' VSS team. ITAndroids is a robotics re- Kalman filter in the described VSS tracking prob- search group at Technological Institute of Aero- lem, as well as assessing its performance based on nautics which was refounded in mid-2011 after the Root Mean Square Error (RMSE) calculated some few years of inactivity. The group par- for several simulations and also applying it to real ticipates in several national and international data. In this work, Section 1 gives a presentation competitions. In particular, ITAndroids' VSS and an overview of the subject, Section 2 describes team has participated in the 2014 and 2015 edi- the mentioned techniques introducing the corre- tions of the Latin American Robotics Competi- spondent algorithms and Section 3 provides the tion (LARC) and Brazilian Robotics Competition stochastic model of the differential drive robots (CBR), which happen as one single event, achiev- in the tracking problem. Section 4 describes the ing the 4th place in the latest edition. procedure used in the covariance calibration pro- ISSN21758905 1520 XIII Simp´osio Brasileiro de Automa¸c˜ao Inteligente Porto Alegre – RS, 1o – 4 de Outubro de 2017 cess. Section 5 presents some implementation re- Extended Kalman Filter, in contrast with the lin- sults using the vision data from a real robot and ear one, may not be optimal and may produce un- also some simulation tests. Finally, in Section 6 desirable results for highly non-linear functions. we can see the main conclusions and final consid- For smooth non-linear functions, in which the erations. linearization gives a good local approximation of the function, the Extended Kalman Filter usu- 2 Probabilistic Localization ally has good performance. Equations (3) to (6) show precisely how the linearization is performed. In the state-space and discrete time approach to The complete EKF algorithm is presented in Al- the tracking problem, we represent the robot's gorithm 1. pose at time k 2 by its state vector x 2 n. Z k R @f (x; u ; w) From a probabilistic formulation, we define its F = k k−1 jw=0 (3) k x=xk−1jk−1 state as a vector of random variables, and for rep- @x resenting its state uncertainty we are interested in @fk(x; uk−1; w) w=0 Lk = j (4) the probability density function (pdf) of xk. @w x=xk−1jk−1 In order to use the concept of bayesian fil- @h (x; v) tering, two models are needed (Arulampalam H = k jv=0 (5) k x=xkjk−1 et al., 2001). A model describing the dynamic @x evolution of the state, called the movement model, @hk(x; v) v=0 Mk = j (6) represented in equation (1), and a second model @v x=xkjk−1 relating the noisy measurement to the state, called the measurement model, represented in equation Algorithm 1 EKF (2). Initialization x^0j0 = x^0 xk = f(xk−1; uk−1; wk−1) (1) P0j0 = P0 for k 1 to N do zk = h(xk; vk) (2) In equation (1), f is the propagation function, Prediction Step u is the vector of control commands and w is the k k x^kjk−1 fk(x^k−1jk−1; uk−1; 0) process associated random perturbations. Mean- @fk Fk jx=x^ while, in equation (2), h is the function that trans- @x k−1jk−1 L @fk j forms the state space into the observation space, k @w x=x^k−1jk−1 T T Pkjk−1 FkPk−1jk−1F + LkQkL with an associated random noise vk. We shall con- k k sider that wk and vk are independent and have Filtering Step null mean and covariance matrices Qk and Rk, @hk Hk jx=x^ respectively. The estimate of xk will be denoted @x kjk−1 @hk by x^kjk and its covariance matrix by Pkjk. Mk @v jx=x^kjk−1 T T Given these models, the Bayesian approach Sk HkPkjk−1Hk + MkRkLk T −1 aims to construct the posterior state distribution Kk Pkjk−1Hk Sk function p(xkjz1:k) given all the current measure- x^kjk x^kjk−1 + Kk(zk − h(x^kjk−1)) ments up to k. In the next subsections, brief re- Pkjk (I − KkHk)Pkjk−1 views of two famous techniques widely used in the attempt to construct such distribution are pre- end for sented. They are both non-linear extensions of the well known Kalman Filter, which provides the optimal estimator in the case in which the equa- 2.2 Unscented Kalman Filter tions (1) and (2) are linear and the noises and the initial distribution are Gaussian and mutually The EKF may perform poorly when the predic- independent. tion and observation functions are highly nonlin- ear. The Unscented Kalman Filter (UKF) uses a deterministic sampling technique, known as the 2.1 Extended Kalman Filter unscented transform (UT). This method better The Extended Kalman Filter (EKF) extends the approximates the posterior's true mean and co- original Kalman Filter by linearizing the models variance (Julier and Uhlmann, 2004). Therefore, given by equations (1) and (2) around the current the UKF often provides better estimate than the estimate. The extended Kalman Filter, despite EKF with similar computational cost. Addition- the non-linearities, still assumes that the state ally, the UKF does not require Jacobians com- probability function is Gaussian, even though that putations, which may be a hard task for very is not true in general for non-linear functions. The complex models. The UT carefully picks samples 1521 XIII Simp´osio Brasileiro de Automa¸c˜ao Inteligente Porto Alegre – RS, 1o – 4 de Outubro de 2017 (called sigma points) around the mean and prop- Algorithm 3 Function DrawSigmaPoints. agate them through the nonlinear function, from function DrawSigmaPoints(xa, Pa) which the mean and covariance of the estimate χ(0) = xa (0) λ are computed. Algorithm 2 shows a pseudocode wm = for the UKF, where the sigma points are selected n+λ w(0) = λ + 1 − α2 + β using the function DrawSigmaPoints, presented in c n+λ for i = 1; : : : ; n do Algorithm 3. χ(i) = xa + p(n + λ) Pa In the function DrawSigmaPoints, α and β :;i (i) (i) 1 are constants, where α determines the spread of wm = wc = 2(n+λ) the sigma points around the mean and is usually end for set to a small value (we use α = 10−3) and β = for i = n + 1;:::; 2n do 2 is the optimal value for gaussian distributions.