Neural Network-Aided Extended Kalman Filter for SLAM Problem

Transcription

1 7 IEEE International Conference on Robotics and Autoation Roa, Italy, -4 April 7 ThA.5 Neural Network-Aided Extended Kalan Filter for SLAM Proble Minyong Choi, R. Sakthivel, and Wan Kyun Chung Abstract This paper addresses the proble of Siultaneous Localization and Map Building (SLAM) using a Neural Network aided Extended Kalan Filter (NNEKF) algorith. Since the EKF is based on the white noise assuption, if there are colored noise or systeatic bias error in the syste, EKF inevitably diverges. The neural network in this algorith is used to approxiate the uncertainty of the syste odel due to isodeling and extree nonlinearities. Siulation results are presented to illustrate the proposed algorith NNEKF is very effective copared with the standard EKF algorith under the practical condition where the obile robot has bias error in its odeling and environent has strong uncertainties. In this paper, we propose an algorith which enables a biased control input in vehicle odel using neural network. I. INTRODUCTION In robotics, the proble of siultaneous localization and ap building is that of estiating both a robot s position and a ap of its surrounding environent. In general, it is a coplex proble because noise in the estiate of the robot s pose leads to uncertainty in the estiate of the ap and vice versa. The Extended Kalan Filter (EKF) has becoe a standard technique used in siultaneous localization and ap building proble. During the past few years significant progress have been ade towards the solution for the SLAM proble [3], [4], [6] with the aid of EKF. EKF assues a white Gaussian noise in prediction and easureent odel. In real situation, however, there are biased systeatic error in obile robot even after appropriate copensation [5]. If there are also soe colored noise in control input or drifting errors in dead-reckoning, they could affect the quality of SLAM directly. In addition to these, a priori knowledge of the plant odel is required to copute both the prediction of the state estiate and its Jacobian. Often, the plant odel is not copletely known due to isodeling, extree nonlinearities and changes in syste para. To perfor state estiation when such conditions occur, Martinelli et al [] introduced an augented Kalan filter which siultaneously estiates the robot configuration and the para characterizing the systeatic error. In this paper, we propose to ipleent an EKF whose plant odel is augented by an Neural Network(NN). The NN will capture the unodeled dynaics by learning on line. The function describes the difference between the true plant and the best odel. The processing of iprecise or noisy data by the neural networks is ore efficient than classical techniques because neural network is highly tolerant to noises [7]. As a relevant researches in [8], [9], the extended Kalan filter has been used to train the ultilayer perception network by treating the weights of a network as the state of the nonlinear dynaic syste. In [3] dual extended Kalan filter is presented for reoving nonstationary and colored noise fro speech. More recently, Zhan and Wan [4] developed NN-aided adaptive unscented Kalan filter for nonlinear state estiation. Even though the above works studied for state estiation proble, no work have been reported regarding the SLAM proble with the aid of neural network. Motivated by the above considerations, the goal of this paper is to introduce a neural network based extended Kalan filter to SLAM proble to deal with systeatic error. To the best of authors Knowledge, this is possibly the first attept to this proble. The NNEKF is a cobination of syste identification and standard EKF. In [9], two EKFs are coupled so that a single EKF will be used siultaneously to estiate the state and train the weights of the NN. For siplicity, in this paper we consider a two hidden layer neural network. This paper is organized as follows. In section II, a neural network aided extended Kalan filter is explained briefly. Section III describes about SLAM proble using NN-aided EKF. In Section IV, siulation results are provided to deonstrate the effectiveness of the proposed algorith. Finally, Section V contains soe concluding rearks. II. NN-AIDED EXTENDED KALMAN FILTER A. Neural Network Neural network is a network of single perceptron. The single perceptron is represented as in Fig.. According to inputs x i and weights a ij, output y j is changed. The function f( ) is referred to as the squashing function, one of which a sigoid type function is widely used since its derivative is easily obtained. Minyong Choi, R. Sakthivel, and Wan Kyun Chung are with Robotics and Bio-echatronics Lab., Departent of Mechanical Engineering, Pohang University of Science and Technology (POSTECH), Pohang, Korea {inyong,sakthi,wkchung}@postech.ac.kr Fig.. Single Perceptron /7/$. 7 IEEE. 686

2 ThA.5 Fig. 3. Neural Network-Aided Extended Kalan Filter Fig.. A Siple Neural Network Structure f(θ) = + +e θ () f (θ) = ( f(θ)) () covariance and W represents the Kalan gain. Roughly, NNEKF is shown in Fig. 3. In real situation, true odel f(x k,u k+ ) is usually unknown quantity. We denote E as the error function between true odel and a priori known atheatical odel ˆf(x k,u k+ ), naely, A siple neural network which has one hidden layer represented in Fig.. Outputs of network are related with inputs, connecting weights, and apping function like as sigoid. Each output of NN can be written as the following for ( J I ) y k = a jk f a ij x i (3) j= i= B. NN-Aided Extended Kalan Filter Lobbia et al. [9] developed an adaptive state estiation technique using NN and they used an EKF to estiate the states by using a dynaic syste odel, at the sae tie, usingtheekftotrainthenn.thennisusedtoiprove the learning and adaptation capabilities related to variations in the environent where the inforation is inaccurate, uncertain and incoplete. The plant s odel, and observation odel are governed by the nonlinear dynaic syste x k+ = f(x k, u k+ )+w k+ (4) z k = h(x k )+v k in which the input vector u k is the control coand, and w k+ and v k represent process noise and observation noise with zero ean and covariances atrices Q and R respectively. The standard EKF algorith to estiate the states of the syste described in (4) is given by the following equations ˆx k+ k = f(ˆx k k, u k+ ) P k+ k = x f P k k x f T + Q k+ S k+ = x h P k+ k x h T + R k+ W k+ = P k+ k x h S ˆx k+ k+ = ˆx k+ k + W k+ (z h(ˆx k+ k )) P k+ k+ = P k+ k W k+ S k+ Wk+ T where ˆx eans estiated states, z represents real easureents, the subscript k is the discrete-tie index, P is estiated error covariance of states, S eans innovation E = f(x k, u k+ ) ˆf(x k, u k+ ) (5) The NN is used to approxiate the error. Obviously if the error E is sall, the state estiate fro the NNEKF will be better. We can estiate the error E using a siple feedforward neural network g(x k, u k+, a k ), a represent neural network weights. When E g(x k, u k+, a k ), the error is well approxiated and the ore accurate odel is defined as [ ] [ ] xk+ f(xk, u = k+ )+g(x k, u k+, a k ) (6) a k+ Now the proble becoes the state estiation based on the above new odel and the noisy observation. Now the filtering procedure of NN-aided EKF can be carried out in the siilar way of EKF algorith. Note: An iportant advantage of NN-EKF is that the weight is learned during the update, since augented state vector contains neural networks weight. In this paper, as discussed in [8], [9], [], two EKFs are coupled into a single EKF which does the double duty by siultaneously estiating the state of the syste and training the weights. a k III. SLAM USING NNEKF SLAM is the process of siultaneously building up a ap of the environent and using this ap to obtain estiates of the vehicle pose. The vehicle is equipped with an onboard sensor to easure relative distance and bearing angle of the vehicle to the environent. As the obile robot integrate its pose using dead reckoning, it refines its own pose and feature s position fro sensor data without absolute inforation. Since there is noise in odoetry inforation and sensor data, SLAM algorith requires a quantity to realize the noise like error covariance in EKF. 687

3 ThA.5 A. SLAM using standard EKF We consider D SLAM proble, EKF states to solve SLAM are defined as vehicle s pose and features position; x v x f x =., x v = x v [ ] y v xfi, x fi = (7) y θ fi v x fn where (x, y) is position, and θ is heading of vehicle. The estiated error covariance is defined as, P vv P vf P vfn P fv P ff P ff N P = P fn v P fn f P fn f N where the diagonal sub-atrices are covariance of vehicle and each feature, and off-diagonal sub-atrices are correlation of the. A detailed SLAM algorith using standard EKF is presented in [6]. B. SLAM using NNEKF In this section, the NNEKF is used to study the SLAM proble. A ain advantage of the NNEKF is that it is used siultaneously for state estiation (both vehicles position and feature position) and neural network training. If we represent the augented state vector as x =[x T v then (6) can be redefined as x T a x T f ]T x v,k+ f v (x v,k, u k+ )+g(x v,k, x a,k, u k+ ) x a,k+ = x f,k+ x a,k x f,k where x a is neural network s weight. In general, feature s position in SLAM proble is assued to be stationary. It is noted that in the prediction odel, neural network weights are not changed as like feature s position. The plant Jacobian with respect to states is given by x v f v + xv g xa g x f = I f (9) I a In the above Jacobian xv g, and xa g are depending on neural network structure. The plant Jacobian with respect to control inputs is given by u f + u g u f = () The augented state vector estiates the state estiate and weights of the neural networks siultaneously. The NNEKF algorith for SLAM proble is described. ) Initialize x v =, P vv = x a = ɛ, P aa = λi (8) ) Prediction ˆx v,k+ = f v(ˆx v,k, u k+ )+g(ˆx v,k, ˆx a,k, u k+ ) ˆx a,k+ ˆx f,k+ ˆx a,k ˆx f,k P k+ k = x f P k k x f T + u f Q u f T 3) Data Association 4) Update x h = [ xv h a xf h ] S k+ = x h P k+ k x h T + R k+ W k+ = P k+ k x h S ˆx k+ k+ = ˆx k+ k + W k+ (z h(ˆx k+ k )) P k+ k+ = P k+ k W k+ S k+ Wk+ T 5) New Feature Augentation IV. SIMULATION RESULT In this section, we apply the proposed NNEKF algorith to SLAM proble and copare it with the standard EKF algorith. To test the perforance of our algorith, we use the Ackeran vehicle odel, in which control inputs are linear velocity and steering angle. x k+ x k + δtv k+ cos(θ k ) y k+ = y k + δtv k+ sin(θ k ) () θ k+ θ k +(δtv k+ tan(φ k+ ))/L where V is linear velocity, φ is steering angle, L =.5, and δt =.5 s. Matlab siulation code developed by Bailey et al. [] for SLAM which is opened on the web-site [5]. We odified their siulation code according to our work and used to verify the algorith. The vehicle s unbiased control input has axiu linear velocity as. /s, and axiu steering angle as 3. Noise of control input is assued zero ean Gaussian with σ V =./s, and σ φ =. To siulate biased control input, we add linear velocity by.5 /s, and steering angle by.. This coes fro the following reasoning. Suppose both wheel dia are c, the change of.5 c of the wheel diaeter can ake the.5 /s biases in linear velocity. As the wheel tread is 4 c and the difference between two wheel dia is. c, nearly. biases could be occurred in steering angle. The environent is relatively large about. Since the sufficient nuber of features are required to guarantee observability, we used about features and there is one feature in each area. In the observation odel, range-bearing sensor odel are used to easure the feature position and vehicle pose related to its ap, whose noise level is. in range, in bearing. The sensor range is restricted under, which can detect all features in front of vehicle ultilayered neural network having two hidden layers with sigoid function is used in the siulation. Since the input of the neural network is the augented vector of 688

4 ThA (a) (EKF) (b) (NNEKF) Fig. 4. Siulation Result in Unbiased Case; black dash-dot line: reference trajectory, black (+): true features; blue solid line: estiated trajectory by NNEKF, blue (+): estiated features by NNEKF; blue ellipse: estiated covariance of vehicle and feature by NNEKF; green solid line: estiated trajectory by standard EKF, green (+): estiated features by standard EKF; green ellipse: estiated covariance of vehicle and feature by standard EKF (c) (EKF) (d) (NNEKF).. the vehicle pose and control inputs, the nuber of input neurons is 5. If there is only one hidden layer, an interaction between neurons akes it difficult to learn. So, in practical considerations, two hidden layers are ore anageable. The nuber of neurons of output layer is deterined by the diension of the vehicle pose. Total nuber of weights are 33, whose initial value is not zero but sufficiently sall value. Data association is also a very iportant part in SLAM proble [], and we use a siple nearest neighbor technique since there are sufficient sparseness in features. In Fig. 4 and Fig. 6, black dash-doted line is the reference trajectory of the vehicle and black crosses are true feature positions. To represent NNEKF case, blue solid line for the estiated trajectory and blue crosses for the estiated feature positions are used. Green solid line for the estiated trajectory and green crosses for the estiated feature positions are used in standard EKF case. Also, blue and green ellipse represent covariance of vehicle and features. For unbiased control input case, both results are alost the sae as shown in Fig. 4, since the noise is white. The vehicle s pose error is plotted in Fig. 5, which shows the consistency of both estiation algorith. Fig. 6 shows the biased input case. Initially, both EKF and NNEKF results are alost the sae, until neural network s weights are trained. However, as tie increases, neural network s weights are trained, significant difference appeared. The error between the true vehicle pose and an estiated vehicle pose is plotted in Fig. 7. In NNEKF, initially the error exceeded σ covariance bound, but after soe tie vehicle s position error is bounded by σ asshowninfig. 7(b) and Fig. 7(d). In EKF, however, the error is not bounded as shown in Fig. 7(a) and Fig. 7(c) after 5. The result clearly shows that NNEKF is superior to EKF in biased vehicle odel condition (e) heading error (EKF) (f) heading error (NNEKF) Fig. 5. Vehicle Pose Error in Unbiased Model; red lines represent σ bound and blue line represents error of each pose V. CONCLUSIONS This paper presents a neural network aided extended Kalan filter approach to siultaneous localization and ap building proble. As the NN is trained online, NNEKF can capture the unodeled dynaics and adapt to the changed condition intelligently. The NNEKF was shown to have superior perforance with biased vehicle odel. Siulation data was used to provide coparison between NNEKF and standard EKF for SLAM proble. The siulation results deonstrate that the proposed NNEKF algorith is very effective in SLAM proble with biased vehicle odel. It is noted, however, that NNEKF and EKF alost have the sae perforance with unbiased vehicle odel. VI. ACKNOWLEDGMENTS This research was supported in part by the National Research Laboratory (NRL) Progra(M J--4-) of the Ministry of Science and Technology, by a grant(a-6-6) of the Ministry of Inforation and Counication, and by a grant(-pj3-pg6-ev4-3) of Ministry of Health and Welfare, Republic of Korea. REFERENCES [] C. M. Bishop, Neural Networks for Pattern Recognition, Oxford University Press Inc., New York,

5 ThA (a) (EKF) (b) (NNEKF) (a) NNEKF (c) (EKF) (d) (NNEKF) (e) heading error (EKF) (f) heading error (NNEKF) (b) Standard EKF Fig. 6. Siulation Result in Biased Case; black dash-dot line: reference trajectory, black (+): true features; blue solid line: estiated trajectory by NNEKF, blue (+): estiated features by NNEKF; blue ellipse: estiated covariance of vehicle and feature by NNEKF; green solid line: estiated trajectory by standard EKF, green (+): estiated features by standard EKF; green ellipse: estiated covariance of vehicle and feature by standard EKF. [] T. Bailey, J. Neito, J. Guivant, M. Stevens, and E. Nebot, Consistency of the EKF-SLAM Algorith, Proceedings of the 6 IEEE/RSJ International Conference on Intelligent Robots and Systes, 6, pp [3] J. A. Castellanos, J. M. M. Montiel, J. Neira and J.D. Tardos, The SPap: A Probabilistic fraework for Siultaneous Localization and Map Building, IEEE Transcations on Robotics and Autoation, Vol. 5, 999, pp [4] A. Dissanayake, P. Newan, S. Clark, H. Durrant-Whyte and M. Csorba, A Solution to the Siultaneous Localization and Map Building Proble, IEEE Transcations on Robotics and Autoation, Vol. 7,, pp [5] N. L. Doh, H. Choset, and W. K. Chung, Accurate Relative Locaization Using Odoetry, Proceedings of the 3 IEEE International Conference on Robotics and Autoation, 3, pp [6] H. Durrant-Whyle and T. Bailey, Siultaneous Localization and Mapping: Part I, IEEE Robotics and Autoation Megazine, Vol. 3, 6, pp [7] S. Haykin, Kalan Filtering and Neural Networks, John Wiley and Fig. 7. Vehicle Pose Error in Biased Model; red lines represent and blue line represents error of each pose Sons, New York,. [8] Y. Iiguni, H. Sakai and H. Tokuaru, A real-tie Learning Algorith for a Multilayered Neural Network Based on the Extended Kalan Filter, IEEE Transcations on Signal Processing, Vol. 4, No. 4, 99, pp [9] R. N. Lobbia, S. C. Stubberud, and M. W. Owen, Adaptive Extended Kalan Filter Using Artificial Neural Networks, International Journal of Sart Engineering Syste Design, Vol., 998, pp. 7-. [] A. Martinelli, N. Toatis, A. Tapus, and R. Siegwart, Siultaneous Localization and Odoetry Calibration for Mobile Robot, Proceedings of the 3 IEEE/RSJ International Conference on Intelligenct Robots and Systes, 3, pp [] J. Neira and J. D. Tardós, Data Association in Stochastic Mapping Using the Joint Copatibility Test, IEEE Transactions on Robotics and Autoation, Vol. 7, No. 6,, pp [] D. W. Ruck, S. K. Rogers, M. Kabrisky, P. S. Maybeck, and M. E. Oxley, Coparative Analysis of Backpropagation and the Extended Kalan Filter for Training Multilayer Perceptrons, IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 4, No. 6, 99, pp [3] E. A. Wan and A. T. Nelson, Neural Dual Extended Kalan Filtering: Applications In Speech Enhanceent And Monaural Blind Signal Separation, Proceeding of IEEE workshop on Neural Networks for Signal Processing, 997, pp [4] R. Zhan and J. Wan, Neural Network-Aided Adaptive Unscented Kalan Filter for Nonlinear State Estiation, IEEE Signal Processing Letters, Vol. 3, No. 7, 6, pp [5] tbailey/sofware/index.htl 69