A Fuzzy Kalman Filter Approach to the SLAM Problem of Nonholonomic Mobile Robots

Transcription

1 Proceedings of the 17th World Congress The International Federation of Automatic Control Seoul, Korea, July 6-11, 8 A Fuzzy Kalman Filter Approach to the SLAM Problem of Nonholonomic Mobile Robots Keigo Watanabe, Chandima Dedduwa Pathiranage, and Kiyotaka Izumi Department of Advanced Systems Control Engineering, Graduate School of Science and Engineering, Saga University, 1-Honjomachi, Saga 8-85, Japan. (Tel : ; watanabe@me.saga-u.ac.jp, dpchandima@yahoo.com, izumi@me.saga-u.ac.jp) Abstract: This paper presents an alternative solution to simultaneous localization and mapping (SLAM) problem by applying a fuzzy Kalman filter using a pseudolinear measurement model of nonholonomic mobile robots. Takagi-Sugeno fuzzy model based on an observation for a nonlinear system is adopted to represent the process and measurement models of the vehicle-landmark system. The complete system of the vehicle-landmark model is decomposed into several linear models. Using the Kalman filter theory, each local model is filtered to find the local estimates. The linear combination of these local estimates gives the global estimate for the complete system. The simulation results shows that the new approach performs better, though nonlinearity is directly involved in the Kalman filter equations, compared to the conventional approach. 1. INTRODUCTION The simultaneous localization and mapping (SLAM) problem (Durrant-Whyte & Bailey, 6), also known as concurrent mapping and localization (CML) problem, is often recognized in the robotics literature as one of the key challenges in building autonomous capabilities for mobile vehicles. The goal of an autonomous vehicle performing SLAM is to start from an unknown location in an unknown environment and build a map (consisting of environmental features) of its environment incrementally by using the uncertain information extracted from its sensors, whilst simultaneously using that map to localize itself with respect to a reference coordinate frame and navigate in real time. The first solution to the SLAM problem was proposed by Smith et al. (1987). They emphasized the importance of map and vehicle correlations in SLAM and introduced the extended Kalman filter (EKF)-based stochastic mapping framework, which estimated the vehicle pose and the map feature (landmark) positions in an augmented state vector using second order statistics. Although EKF-based SLAM within the stochastic mapping framework gained wide popularity among the SLAM research community, over time, it was shown to have several shortcomings (Leonard & Durrant-Whyte, 1991; Dissanayake et al., 1). Notable shortcomings are its susceptibility to dataassociation errors and inconsistent treatment of nonlinearities. Here we propose some remedies to overcome the shortcomings of EKF algorithm. To preserve the nonlinearity in the system, motion and observation models are represented by the pseudolinear models (Li & Jikov, 1; Whitcombe, 197; Watanabe, 1991). This avoids the direct linearization of the system. Discrete time motion model is derived from the dead-reckoned measurements of the vehicle pose as to reduce the error associated with the control inputs. This assures the less error prone motion model producing faster convergence. We propose a fuzzy Kalman filter based state estimation algorithm to the SLAM problem. Fuzzy logic has been a promising control tool for the nonlinear systems. Fuzzy state estimation is a topic that has received very little attention. Fuzzy Kalman filtering (Chen et al., 1998) is a recently proposed method to extend Kalman filter to the case where the linear system parameters are fuzzy variables within intervals. We show the superiority of fuzzy Kalman filtering for the state estimation through the SLAM algorithm developed with T-S fuzzy model in this paper. The proposed T-S fuzzy model (Takagi & Sugeno, 1985) based algorithm to the SLAM problem has shown that a demanding (not conventional) solution to the SLAM problem exists and it overcomes limitations of the EKF based SLAM hinting a new path explored is much suitable in finding an advanced solution to localization and mapping problems.. VEHICLE MODEL AND ODOMETR In the history of SLAM problem, it has been the common practice of generating the motion model with forward velocity and steering angle as control inputs. In this representation, measurement errors in control inputs propagate into the next stage with the same noise strength. But the model that we present has less error prone control inputs as control inputs to the motion model are derived from the successive deadreckoned poses where current dead-reckoned pose subtracts the immediate previous dead-reckoned pose to produce the control input and it is hopeful that this subtracts the common deadreckoned error giving a control input with low noise level..1 Dead-Reckoned Odometry Measurements Assume left and right wheels of radius r mounted on both sides of the rear axle turn amounts δθ l and δθ r in one time interval, as shown in Fig. 1. We want to express the change of position of the center of rear axle of the vehicle (δx o,δy o ) and the change of orientation (δφ o ) as a function of δθ l and δθ r.fromthe geometrical relationship of Fig. 1, it is easy to see that rδθ r =(c L/)α, rδθ l =(c + L/)α (1) /8/$. 8 IFAC /876-5-KR-11.9

2 17th IFAC World Congress (IFAC'8) Seoul, Korea, July 6-11, 8 x o 1 ø o y o 1 x o (k) u o (k) r L c x o (k-1) Fig. 1. Geometric construction of rear wheel movements Solving above two equation in (1) for c and α, we obtain c = L δθ l + δθ r, α= r δθ l δθ r L (δθ l δθ r ) () immediately then it yields that [ ] [ ] δxo (1 cosα)c δy o δφ o = c sinα α The dead-reckoning system in the vehicle simply compounds these small changes in position and orientation to obtain a global position estimate. Figure shows a vehicle with a prior pose x o (k 1). The processing of wheel rotations between successive readings (via (3)) has indicated a vehiclerelative transformation (i.e. in the frame of the vehicle) u o = [δx o,δy o,δφ o ] T. The task of combining this new motion u o (k) with the old dead-reckoned estimate x o (k 1) to arrive at a new dead-reckoned posed x o (k) is trivial, i.e., x o (k)=x o (k 1) u o (k) () We want to figure out the control inputs to the vehicle motion model (u v =[δx,δy,δφ] T ) from the successive dead reckoned poses. Compounding x o (k) to inverse relationship of x o (k 1) results in u v (k) and is given by u v (k)= x o (k 1) x o (k) (5) We are now in a position to write down the vehicle motion model using dead-reckoned poses as a control input: x v (k +1)=f (x v (k), u v (k)) = x v (k) ( x o (k 1) x o (k)) (3) = x v (k) u v (k) (6) 3. PSEUDOLINEAR SSTEM MODELING In the following, the vehicle state is defined by x v =[x, y, φ] T. where x and y are the coordinates of the center of the rear axel of the vehicle with respect to some global coordinate frame and φ is the orientation of the vehicle axis. The landmarks are modeled as point landmarks and represented by a Cartesian pair such that m i =[x i,y i ] T,i =1,..., N. Both vehicle and landmark states are registered in the same frame of reference. 3.1 The Pseudolinear Process Model Figure 3 shows a schematic diagram of the vehicle in the process of observing a landmark. The dead-reckoned measurements obtained from successive vehicle frames can be used to World coordinate frame Fig.. Deducing a new dead-reckoned state from a prior deadreckoned state with a local odometry measurement predict the vehicle state from the previous state. The discretetime vehicle process model can be obtained according to the (6) and expressed in the following form: [ ] x(k +1) y(k +1) = φ(k +1) [ x(k) y(k) φ(k) + ] [ cos(φ(k)) sin(φ(k)) sin(φ(k)) cos(φ(k)) 1 ][ ] δx(k) δy(k) (7) δφ(k) which can be represented by the discrete-time pseudolinear vehicle motion model expressed: x v (k +1)=x v (k)+b v (k)u v (k) (8) for use in the prediction stage of the vehicle state estimator. The landmarks in the environment are assumed to be stationary point targets. The landmark process model is thus [ ] [ ] xi (k +1) xi (k) = (9) y i (k +1) y i (k) for all landmarks i =1,..., N. Equation (7) together with (9) defines the process model of the vehicle-landmarks. To represent the process model in the proposed SLAM algorithm, the vehicle-landmarks augmented state vector can then be represented in the following pseudolinear form: x(k +1)=x(k)+B(k)u(k) (1) where x(k) =[x T v (k) m T (k)] T, B(k) = [ ] Bv T (k) T T 1 and u(k)=u v (k), inwhich 1 is a null matrix. 3. The Observation Model Range r i (k) and two bearing measurements θ i 1 (k) and θi (k) to landmark i are recorded by the range and bearing sensors. The range measurements and bearing measurements are taken from the center of rear vehicle axel where the vehicle position (x, y) is taken. One sensor starts reading measurements from the x axis and the other from the center axis of the vehicle. Referring to Fig. 3, the observation model for ith landmark z i (k) =[r i (k),θ i (k),β i (k)] T can be written in direct form as r i (k)= (x i x(k)) +(y i y(k)) + v r (k) (11) ( ) θ i (k)=θ1(k) i yi y(k) = arctan + v θ (k) (1) x i x(k) 61

3 17th IFAC World Congress (IFAC'8) Seoul, Korea, July 6-11, 8 (x, y) (x i, y i ) r i i θ i θ 1 World coordinate frame Fig. 3. Vehicle-landmark model ( ) θ(k) i yi y(k) = arctan φ(k)+v θ (k) (13) x i x(k) β i (k)=θ1 i (k) θi (k)=φ(k)+v β(k) (1) where v r and v θ are the white noise sequences associated with the range and bearing measurements with zero means and standard deviations σ r and σ θ respectively. v β is also assumed to be white with zero mean and standard deviation σ β.the covariance matrix Rz for the observation model given by (11), (1) and (1) is then in the form: Rz = φ σ r σ θ σ β γ V (15) Thus, (11), (1) and (1) define the observation model for the ith landmark. Pseudolinear Observation Model: In this section, we present the pseudolinear measurement model. The pseudomeasurement method (Aidala, 1985) relies on representing the nonlinear measurement model given by (11), (1) and (1) in the following pseudolinear form: y(z)=h(z)x + vy(x, v) (16) where the pseudomeasurement vector y(z) and matrix H(z) are known functions of the actual measurement z. v = [v r,v θ ] T is the range and bearing measurement noise vector and vy(x, v) is the corresponding pseudomeasurement error, now state dependent. The underlying idea of the approach is clear. Once a pseudolinear model (16) is available, a linear Kalman filter can be readily used with y(z), H(z), and Ry(x )= cov[vy(x, v)], where a common choice of x is the predicted state estimate ˆx. Equations (11), (1) and (1) can be rearranged by algebraic and trigonometric manipulations to obtain the following model expressed by r i (k)=(x i x(k))cos(θ i (k)) + (y i y(k))sin(θ i (k)) +v r (k) (17) =(x i x(k))sin(θ i (k)) (y i y(k))cos(θ i (k)) +v y θ (k) (18) β i (k)=φ(k)+v β (k) (19) where v y θ (k)=r i,true(k)v θ (k). The composite model of above three equations can be expressed in the following pseudolinear form for the ith landmark: where y(z i )= H(z i )= [ ] ri (k) = H(z i )x + vyi(x, v) () β i (k) [ λcos(θi (k)) λsin(θ i (k)) λsin(θ i (k)) λcos(θ i (k)) 1 λcos(θ i (k)) λsin(θ i (k)) λsin(θ i (k)) λcos(θ i (k)) ] (1) λ =1 exp( σθ )+exp( σ θ /) () where λ is the debiased conversion factor obtained by the nested conditioning of state covariance (Li & Jikov, 1). This conversion serves to compensate the estimation bias and process measurement components. vy(x, v) is considered to be white and its covariance is expressed by Ryi(ˆx)= σ r ˆr i σ θ σ β (3) Note that, ˆr i is used in calculating Ryi because r i,true is not available.. TAKAGI SUGENO (T-S) FUZZ MODEL The fuzzy model proposed by Takagi and Sugeno is described by fuzzy IF-THEN rules, which represent local linear inputoutput relations of a nonlinear system. The jth rule of the T-S fuzzy model is of the following form: Rule j : IF q 1 (k) is F j1 and and q g (k) is F jg THEN x(k +1)=A j x(k)+b j u(k) y(k)=c j x(k) j =1,,,r. () F jl is the fuzzy set and r is the number of IF-THEN rules. x(k) R n is the state vector, u(k) R m is the input vector, y(k) R p is the measurement vector. Given a pair of (x(k), u(k)), the final outputs of the fuzzy systems are inferred as follows: r x(k +1)= w j(q(k)){a j x(k)+b j u(k)} r w j(q(k)) where = r h j (q(k)){a j x(k)+b j u(k)} (5) q(k)=[q 1 (k) q g (k)], w j (q(k)) = g F jl (q l (k)) (6) l=1 r w j (q(k)) >, w j (q(k)) forj =1,,,r (7) h j (q(k)) = w j (q(k)) r w j(q(k)) (8) 6

4 17th IFAC World Congress (IFAC'8) Seoul, Korea, July 6-11, 8 π b a y = cosφ (, ) y =1 a y = cosb y = cos a b y = 1 Fig.. Approximation of nonlinear term cosφ for all k. F jl (q l (k)) is the grade of membership of q l (k) in F jl. From (5) (8) we have r h j (q(k)) = 1, h j (q(k)) forj =1,,,r (9) for all k..1 Fuzzy Modeling of Nonlinear Terms Fuzzy description of nonlinear term cosφ can be expressed as follows. It is assumed that φ varies in between π and π. cosφ can be rewritten for two cases by using two linear models for each case. This is illustrated in Fig.. They can be represented as follows: cosφ = F1 1 (φ) 1+F 1 (φ) cosa for φ π/ (3) cosφ = F 1 (φ) ( 1) + F (φ) cosb for π/ < φ <π(31) Here, a = π/ δ, b = π/+δ and δ is a small positive angle. The membership functions in (3) and (31) are defined as F1 1 = {about }, F1 = {about ± a}, F 1 = {about ± π} and F = {about ± b},where F1 1 (φ),f 1 (φ),f1 (φ),f (φ) [, 1] (3) F1 1 (φ)+f 1 (φ)=1, F1 (φ)+f (φ)=1 (33) Solving the above equation gives F1 1 cosφ cosa (φ)= 1 cosa,f 1 (φ)=1 F1 1 (φ)= 1 cosφ 1 cosa (3) F 1 cosb cosφ (φ)= 1+cosb,F (φ)=1 F 1 (φ)= 1+cosφ 1+cosb (35) Inthesameway,sinφ can also be rewritten by the combination of linear models and can be deduced from the above cosφ by the following formula: sinφ =sgn(φ) { 1 if φ> 1 cos φ, sgn(φ)= 1 if φ< (36) 5. FORMULATION OF FUZZ ALGORITHM IN SLAM PROBLEM To reduce the computational cost in using the T-S fuzzy model in SLAM problem, fuzzification of the process model and the pseudolinear measurement model is split into two cases based on value of the vehicle azimuth angle. A set of fuzzy rules is formed for each case and is executed based on the initial separation of vehicle azimuth angle. π φ Case 1: If the azimuth angle (φ(k)) lies between π/ and π/,thejth rule for this case will be of the form: Local linear system rule j relative to the ith landmark: IF φ(k) is F j φ and θ i(k) is F j θ THEN x j (k +1)=x(k)+B j (k)u(k) for j =1,,, 8 y ij (k +1)=H ij (k +1)x j (k +1)+vyij(k +1) (37) F j φ,fj θ {F1 1,F 1,F1,F } are the fuzzy sets of vehicle azimuth angle (φ) and measurement angle (θ i )forthejth rule respectively. B j is the matrix with its nonlinear elements to be sectorial as discussed in fuzzy description of nonlinear terms in the Section.1 and then it becomes a linear matrix for fuzzy sets of vehicle azimuth angle (φ) for each rule in T-S fuzzy model of SLAM problem. In the similar way, the nonlinear elements of the matrices H ij are to be sectorial for fuzzy sets of measurement angle (θ i ). Case : Itisdefinedforπ/ < φ(k) < π and will be composite of eight similar local linear models as defined above. 5.1 Estimation Process In the formulation of T-S fuzzy model based SLAM algorithm, the linear discrete Kalman filter is used to provide local estimates of vehicle and landmark locations for each local linear model defined in T-S fuzzy model. The Kalman filter algorithm proceeds recursively in the three stages: Prediction: The algorithm first generates a prediction for the state estimate, the observation (relative to the ith landmark)and the state estimate covariance at the time k +1for thejth rule according to ˆx j (k +1 k)=ˆx(k k)+b j (k)u(k) (38) ŷ ij (k +1 k)=h ij (k +1)ˆx j (k +1 k) (39) P j (k +1 k)=p(k k)+b j (k)q(k)b T j (k) () Observation: Following the prediction, the observation y i (k +1)of the ith landmark of the true state x(k +1)is made according to (). Assuming correct landmark association, an innovation is calculated for the jth rule as follows: ν ij (k +1)=y ij (k +1) ŷ ij (k +1 k) (1) together with an associated innovation covariance matrix for the jth rule given by S ij (k +1)=H ij (k +1)P j (k +1 k)h T ij (k +1) +R ij (k +1) () Update: The state update and corresponding state estimate covariance are then updated for the jth rule according to ˆx j (k +1 k +1)=ˆx j (k +1 k) +K j (k +1)ν ij (k +1) (3) P j (k +1 k +1)=P j (k +1 k) K j (k +1) S ij (k +1)K T j (k +1) () Here the gain matrix K j (k +1)is given by K j (k +1)=P j (k +1 k)h T ij(k +1)S 1 ij (k +1)(5) 63

5 17th IFAC World Congress (IFAC'8) Seoul, Korea, July 6-11, Landmark locations Decreasing feature uncertainty Vehicle trajectory Covariance in landmark state A e d e Landmark state error s (a) (b) (c) Fig. 5. Feature based map building: The EKF approach Decreasing feature uncertainty Landmark locations Vehicle trajectory Covariance in landmark state d p A p Firstly detected feature Secondly detected feature Thirdly detected feature Fourthly detected feature Fifthly detected feature Sixthly detected feature s (a) (b) (c) Landmark state error Fig. 6. Feature based map building: The pseudolinear model based FKF approach Local state estimates are then combined according to the (5) to obtain the global state estimate for the T-S fuzzy model given by (37). The global estimate is then obtained by the following equation: 8 ˆx(k +1 k +1)= h j (q i (k))ˆx j (k +1 k +1) (6) where q i (k)=[q i1 (k) q i (k)] = [φ(k) θ i (k)]. Propagation of uncertainty for the augmented state error of the T-S fuzzy model is realized by a common covariance that is chosen to be the local covariance which has the minimum trace. This assures the stability of the T-S fuzzy model based SLAM algorithm because it is of paramount importance in state estimation using fuzzy algorithm. The common covariance can be formulated as follows: P(k +1 k +1)=min(trace P j (k +1 k +1)) j (7) The resulting global state estimate and common covariance are then proceeded to the next stage of prediction. Each rule in the T-S fuzzy model takes the global state estimate and the common covariance to generate the next stage prediction. This process is repeated until the required criteria for the state estimation is met. 6. SIMULATION RESULTS In this section, we show the simulation results for the FKF- SLAM algorithm with the measurement model derived from two sensor frames for the system composite of (7), (9) and (). Comparion of performances of the FKF-SLAM algorithm and the EKF-SLAM algorithm was made while keeping all the conditions remain unchanged for the two cases. 6.1 Map Building An environment with six arbitrarily placed landmarks was simulated with a given vehicle trajectory. Simulation results are depicted in Figs. 5 and 6. Figures 5(a) and 6(a) show the evolution of the map over the time obtained from applying the EKF algorithm and the pseudolinear model based FKF algorithm respectively. It can be seen that error ellipses in Fig. 6(a) converge to the actual landmark locations faster than that in Fig. 5(a). This feature can be observed from Fig. 5(b) and Fig. 6(b). A feature that has the same map registration number (where its pose is registered) in the state vector has been indicated in Fig. 5(b) and Fig. 6(b) to compare the performances of uncertainty convergence rate between two methods. The selected feature in Fig. 5(b) is detected at the point A e and it requires d e time span to reach to a minimum bound in uncertainty since detection. And in Fig. 6(b), for the selected landmark, it takes d p time span to reach to a minimum bound in uncertainty since detection at A p. This discloses that the proposed pseudolinear model based FKF approach has higher convergence rate than the EKF approach (d e >d p ).From Fig. 5(c) and Fig. 6(c), it can be observed that the landmark state error obtained from the pseudolinear model based FKF approach reaches to a minimum bound within a less number of time steps compared to that obtained from the EKF algorithm. 6

6 17th IFAC World Congress (IFAC'8) Seoul, Korea, July 6-11, Estimated features Feature uncertainty Robot pose uncertainty Robot x y φ (deg) Standard deviation and error Error in landmark state (a) The FKF-SLAM (b) Standard deviation/error in vehicle pose (c) Landmark location error Fig. 7. Simultaneous localization and mapping Covariance in landmark state Fig. 8. Landmark location covariance 6. Simultaneous Localization and Map Building The newly described method is applied to the feature based- SLAM problem. An environment populated with point landmarks was simulated with the FKF-SLAM algorithm discussed above to generate the state estimates and state errors. Simulation results are depicted in Fig. 7 and Fig. 8. Figure 7(a) shows an instant of the FKF-SLAM algorithm running on the vehicle-landmark systems. It can be seen that error ellipses of the features converge to actual landmark locations as the map of the landmark locations is being constructed when the vehicle navigates through the environment. Figure 7(b) shows standard deviation and error associated with the vehicle pose. It can be seen that the vehicle localization is performed well by the newly presented method as vehicle pose error decreases to a minimum bound gradually. Figure 7(c) shows the evolution of landmark state error. It is observed that the landmark state error obtained from the pseudolinear model based FKF approach reaches to a minimum bound within a less number of time steps compared to that obtained from the EKF algorithm. Figure 8 shows the evolution of the landmark location uncertainty and it can be observed that the landmark location uncertainty gradually decreases over time. It is once shown that the proposed method works well in SLAM problem. 7. CONCLUSION A fuzzy logic and pseudolinear model based solution to the SLAM problem has been proposed in this paper, where the validity of the method was proved with simulation results. The need for direct linearization of nonlinear systems for state estimation is diminished because the newly proposed method performed well and provided a better solution to the SLAM problem. Results obtained from the newly introduced method were compared with those obtained from widely used EKF algorithm to highlight the merit of the pseudolinear model based system with fuzzy logic. It was shown that the pseudolinear model based fuzzy Kalman filter algorithm provided more satisfactory results over the EKF because the pseudolinear models did not lose its nonlinearity when employed in the Kalman filter equations. It was shown that a fuzzy logic based approach with the pseudolinear models provided a remarkable solution to state estimation process because fuzzy logic has been always standing for a better solution. REFERENCES V.J. Aidala. Kalman filter behavior in bearings-only tracking applications. IEEE Trans. on Aerospace and Electronic Systems, 15(1), 9 39, G. Chen, Q. ie, and L.S. Shieh. Fuzzy Kalman filtering. Journal of Information Sciences, 19, 197 9, M.W.M.G. Dissanayake, P. Newman, S. Clark, H.F. Durrant- Whyte, and M. Csorba. A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans. on Robotics and Automation, 17(3), 9 1, 1. H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping: Part I. IEEE Robotics and Automaton Magazine, 13(), 99 18, 6. J.J. Leonard and H.F. Durrant-White. Mobile robot localization by tracking geometric beacons. IEEE Trans. on Robotics and Automation, 7(3), , R. Li and V.P. Jikov. A survey of maneuvering target tracking part III: measurement models. In Proc. SPIE Conference on Signal and Date Processing of Small Targets,San Diego, CA, USA, July-August 1. R. Smith, M. Self, and P. Cheeseman. A stochastic map for uncertain spatial relationships. In Proc. th Int. Symp. Robot. Res., pages. 67 7, T. Takagi and M. Sugeno. Fuzzy identification of systems and its applications to modeling and control. IEEE Trans. Syst., Man, Cybern., 15, , K. Watanabe. Adaptive Estimation and Control: Partitioning Approach. London, UK: Prentice-Hall, D.W. Whitcombe. Pseudo state measurements applied to recursive nonlinear filtering. In Proc. 3rd Symp. Nonlinear Estimation Theory and Its Application, San Diego, CA, pages , Sept