Mobile robot localisation and navigation using multi-sensor fusion via interval analysis and UKF

Transcription

1 Mobile robot localisation and navigation using multi-sensor fusion via interval analysis and UKF Immanuel Ashokaraj, Antonios Tsourdos, Peter Silson and Brian White. Department of Aerospace, Power and Sensors, RMCS, Cranfield University, Shrivenham, Swindon, UK - SN6 8LA I.A.Ashokaraj@rmcs.cranfield.ac.uk Abstract Multiple sensor fusion for robot localisation and navigation has attracted a lot of interest in recent years. This paper describes a sensor based navigation approach using an interval analysis IA based adaptive mechanism for an Unscented Kalman filter UKF. The robot is equipped with inertial sensors INS, encoders and ultrasonic sensors. An UKF is used to estimate the robot position using the inertial sensors and encoders. Since the UKF estimates may be affected by bias, drift etc., an adaptive mechanism using IA to correct these defects in estimates is proposed. In the presence of landmarks the complementary robot position information from the IA algorithm using ultrasonic sensors is used to estimate and bound the errors in the UKF robot position estimate. 1. INTRODUCTION Robot navigation is primarily about guiding a mobile robot to a desired destination or along a pre-specified path in which the robot s environment consists of landmarks and obstacles. In order to achieve this objective the robot needs to be equipped with sensors suitable to localise the robot throughout the path it is to follow. These sensors may give overlapping or complementary information and may also sometimes be redundant. There are many possible architectures to fuse these information. Mobile robots generally carry dead reckoning sensors such as wheel encoders and inertial sensors INS, such as accelerometers, gyroscopes, to measure acceleration and angle rate respectively, and landmark and obstacle detecting and map making sensors such as time of flight TOF ultrasonic sensors. All these sensor measurements can be fused to estimate the robot s position by using a sensor fusion algorithm. Sensor fusion in this case is the method of integrating data from distinctly different sensors to estimate the robot s position. Classical data fusion algorithms use stochastic filters such as Kalman filters for robot position estimation [Jetto et al., 1999]. However the disadvantages of using Kalman filters for robot localisation problems are that the data association step in Kalman filters is complex with ultrasonic sensors and also that, in the case of INS, they are often affected by bias and drift. Moreover an accurate model of the robot system and accurate statistics of the sensor noises are needed, which is not available in many cases. This paper is organised as follows. This introductory section continues by presenting a background for the problem of autonomous robot localisation in section 1.1, followed by a summary of previous work in robot localisation using interval analysis IA in section 1.2. The novelty of the proposed approach is presented in section 1.3. Section 2. explains the implementation of the UKF with INS and encoders for this problem. Section 3. gives a brief explanation of the IA algorithm for robot localisation and also describes how the sensor range limitation is incorporated. In section 4. the implementation of the adaptive mechanism for the UKF robot position estimation using IA with ultrasonic sensors is described and the results are shown and finally in section 5. the conclusions are given. 1.1 Background The problem considered here is that of robot navigation and localisation using multiple low cost sensors such as INS, encoders and ultrasonic sensors. Conventionally stochastic filters such as Extended Kalman filter EKF or Unscented Kalman filters UKF are used for robot localisation [Clark et al., 2001]. One of the main prerequisites for using Kalman filters when using INS and encoders, is to have an accurate model of the robot and also accurate sensor noise statistics. In practice it is difficult to have these parameters accurately, especially the drifts in accelerometers and gyroscopes. This affects the outcome of the UKF, thereby contributing to errors in the estimated position of the robot over a period of time. Moreover TOF sensors such as ultrasonic sensors are used to measure the distance of landmarks from the

2 robot and to recognise the presence of any obstacles in the robots path. When the 2-D map of the environment in which the robot travels is known a priori, the distance measurements from the ultrasonic sensors can be used independently to estimate the position of the robot in the map. Kalman filters can be used for this purpose as well [Castellanos and Tardos, 1999]. But a major limitations encountered of this approach is the problem of data association, as the data association problem in Kalman filters is extremely complex and is of the third order O 3. There are ways in which this problem can be simplified to O 2, but these solutions may be sub-optimal. In order to get the best estimate of the robots position, we can use different types of sensors with different algorithms that, in turn, have different sources of error. In this case we use an Unscented Kalman Filter UKF for fusing the data from the accelerometers, gyroscopes and encoders, instead of the EKF. This is because the UKF can linearise the non-linear models at every instant, up to the 3rd order of Taylor series expansion, thereby reducing the errors during linearisation, where as in the EKF the non-linear models can be linearised only up to the 1st order. In the case of ultrasonic sensors we use an Interval Analysis IA algorithm for estimating the robots position. It should also be noted that the IA algorithm for ultrasonic sensors bypasses the complex data association step and handles the problem in a non-linear way even while being robust to outliners. Thus we have two independent sets of the measurements for the robot position. The estimated robot position using UKF from INS, which might be affected by bias and drift, are then fused with the estimated interval robot position using IA from ultrasonic sensors. The fused robot position estimate is much better than either one them alone, since the errors in UKF estimated position are identified and corrected using the IA algorithm. 1.2 Prior work: Robot localisation with IA using range measurements. Interval analysis is basically about guaranteed numerical methods for approximating sets. Guaranteed in this context means that outer and sometimes inner approximations of the sets of interest are obtained, which can at least in principle, be made as precise as desired. Thus interval computation is a special case of computation on sets, and set theory provides the foundations for interval analysis. The localisation of an autonomous robot while navigating in a known or partially known environment is an important problem in mobile robotics. In this section an approach for the localisation of the robot using IA [Kieffer et al., 2000] with sensor readings from ultrasonic sensors is described briefly. The main advantage of this method is that it bypasses the data-association step, which is very complex in other stochastic methods such as Kalman Filters, and it handles the problem in a xi R θi ỹi si di bj+2 Figure 1: Robot and Sensor model γi aj+2 bj 1 non-linear way without any linearisation and it is very much robust to outliners. The robot model is assumed to move in a known 2D environment and its motion is planned with respect to a set of landmarks. The task now is to estimate the robots position in the map using the distance measurements d m provided by a belt of sensors around the robot. Moreover it is assumed that the bounds on the distance measurement error is know, thus resulting in interval distance measurements. If a model is available to model the ultrasonic sensor interval distance measurements d, the robot localisation problem now becomes a bounded error parameter estimation problem, namely that of characterising the set that contains all the configurations vectors that are consistent with the given map and measurements. The task is to find the configuration vector p where p = x c, y c, θ T and p o is the initial configuration space given by the equation below, aj 1 αidi aj 1 bj aj bj 1 = [p o ] d m 1 [d] 1 i.e. for a given configuration vector p the robot evaluates the measurements that its sensors would return and compares then with the actual measurements to check whether they are consistent. The problem described by the Equation 1 could then be solved using any of the two approaches namely SIVIA Set Inversion Via Interval Analysis [Jaulin and Walter, 1993] and ImageSP [Kieffer et al., 1998]. Both the above approaches have been described in detail in the book by Jaulin et al [Jaulin et al., 2001]. 1.3 Novelty This paper differs from the work in section 1.2 in three main aspects. Firstly, in [Jaulin et al., 2001] it was assumed that the robots TOF sensors had an unlimited range. The algorithm, described in this paper, takes account of the

3 sensor range limitation found in many real world applications, in which case there may be instances during which no landmarks are visible within the robots ultrasonic sensor range. In this paper it is proposed to use inertial sensors INS and encoders when there are no landmark data from the TOF sensors. Secondly, instead of using interval values in the kinematic model while tracking the robot as in [Jaulin et al., 2001], we use the physical constraints of the robot model to predict the robots next position. This approach uses the kinematics of the robot rather than the kinematic model and obviates the need for an accurate kinematic model of the robot that is not always available. This approach also makes the IA algorithm independent of the robot models, thereby reducing sources of uncertainty in the robots estimated position. However we do pay a price in terms of computation time. Finally, in the presence of landmarks, the complimentary robot position estimates are fused using the UKF by converting the intervals into covariance matrix values to represent the uncertainty in the interval robot position, thereby resulting in a much improved robot position estimate from the Unscented Kalman filter UKF. 2. Robot localisation using UKF with Inertial sensors and encoders By fusing the measurement data from the sensors - wheel encoders, gyroscopes and accelerometers - in the mobile robot, a reliable estimation of the position and heading of the robot can be obtained. There are many stochastic filters available for this purpose in the literature such as EKF [Alessandri et al., 1997] etc. In this research work because of the model non-linearity UKF is used instead of an EKF. The main reason for this is that because EKF based on the first order linearisation using the Taylor series expansion, errors in the estimated parameters may be introduced and therefore may result to sub-optimal performance and sometimes divergence of the filter. The problem described above can be overcome to a certain extent by using a method first described by Julier and Uhlman [Julier and Uhlmann, 1997] known as the unscented transform in the Kalman filter for the linearisation process. This formulation of the Kalman filter for a non-linear system is called the Unscented Kalman Filter UKF. The unscented transform is basically a deterministic sampling approach, where the state distribution is approximated by a Gaussian random variable, but is now described using a minimal set of carefully chosen sample points. These sample points are chosen using the unscented transform method that completely describes the true mean and covariance of the Gaussian random variable. When these chosen points are propagated through the true non-linear system, it can capture the posterior mean and covariance accurately up to the 3rd order for Taylor series expansion, where as in a Algorithm d m in: p; out: d m 1 for i := 1 to n s xc cos θ sin θ 2 s i := + s y c sin θ cos θ i 3 cosθ + u 1i := i γ sinθ + θ ; i γ u 2i := cosθ + θ i + γ sinθ + θ i + γ ; 4 d m i p := + ; 5 for j := 1 to n w d m i p := min d m i p, r s i, u 1i, u 2i, a j, b j. Figure 2: Model for calculating the distance expected from ultrasonic sensor when the configuration is p Algorithm [d m] in: [p;] out: [d m] 1 for i := 1 to n s xc cos[θ] sin[θ] 2 [s i ] := + s y c sin[θ] cos[θ] i 3 cos[θ] + [u 1i ] := i γ sin[θ] + θ ; i γ [u 2i ] := cos[θ] + θ i + γ sin[θ] + θ i + γ ; 4 [d m] i [p] := + ; 5 for j := 1 to n w [d m] i [p] := min [d m] i [p],[r] [s i ], [u 1i ], [u 2i ], a j, b j. Figure 3: Inclusion function for the measurement model EKF we can achieve only up to first-order accuracy. It should also be noted that the computational complexity of the UKF is the same order as that of EKF. The basic equations for the UKF has been given in detail in the book [Wan and der Merwe, 2001]. All the process noises are assumed to be zero mean, uncorrelated white random noises only. In the measurement model for this robot there are four sources of observations that are considered: 1. velocity measurements from the wheel encoders, 2. acceleration from the accelerometers, 3. heading angle measurement from the encoders and 4. angular velocity measurements from the rate gyroscope. These measurements are then fused together using the UKF to obtain the speed and heading angle of the robot. 3. Robot localisation with Interval Analysis using range measurements. In this section the robot localisation using IA described briefly in section 1.2 is further elaborated upon.

4 Algorithm [r]in : [s], [ u 1],[ u 2], a, b;out : [r]; ab, 1. [t r] := det as 0 if [t r] = 0 then [r] := + ; return; ab, ba, 2. [t h ] := a [s] 0 b [s] 0 ab, ab, [u1] 0 [u2] 0 ; 3. [r h ] := [χ] [t h ],[l][s],a, b,+ ; 4. [t a] := det [u1], [s]a 0 det [u2], [s]a 0 ; 5. [r a] := [χ] [t a], [s] a,+ ; 6. [t b ] := det [u1], [s]b 0 det [u2], [s] b 0 ; 7. [r b ] := [χ] [t b ], [s] b, + ; 8. for i := 1 to 2 [ ] thi := det [s]a, [ui ] 0 det [s] b, [ui ] 0 ; [ ] [thi ] [ ] rhi := [χ], l [s],a, b,+ ; [ui ] 9. [r] := min [r h ],[r a],[r b ], [ ] [ ] r h1, rh2 ; 10. [r] := [χ] [t r],[r], + Figure 4: Inclusion function for the remoteness of the cone from a segment A brief overview of the algorithm for a single measurement has been given in the Figures 2, 3 and 4. The main improvement in this version of the algorithm is that the range of the ultrasonic sensor has been limited to 3 meters, where as in [Kieffer et al., 2000] the range was unlimited. This is implemented by identifying the sensors n i that only give readings less than 3 meters which is done by setting all the interval ranges greater than 3 meters to infinity in Figure 4 and substituting them for instead of n s where the inclusion function was calculated for all the n s number of sensors in the Figure 3. Also it is assumed that the landmarks are spaced sparsely i.e there is a distance of at least 3 meters between the landmarks. This is because when the land marks are close to one another, some of the land marks may not be visible to the robot sensor model in the IA algorithm in the initial stages of the SIVIA algorithm due to the interval sensor position uncertainty, which results in some feasible robot configurations been eliminated prematurely. Additionally the landmarks which are visible to the robot in the 3 meter radius are only given to the inclusion function in Figure 3 instead of all the n w segments, there by saving computational time.this problem as given in equation 1 is then solved using any of the two approaches namely SIVIA Set Inversion Via Interval Analysis [Jaulin and Walter, 1993] and ImageSP. The Figure 1 gives a brief description of the sensor model and also the measurement process. A detailed description of the above inclusion functions has been provided by [Kieffer et al., 2000]. Also a better version of the above algorithm in terms of computational time, incorporating the interval elementary tests to eliminate some of the infeasible configurations in the configuration vector has been described in [Kieffer et al., 2000], in which the problem is reformulated as P = {p [p o ] tp holdstrue} i.e. P = {p [p o ] tp = 1}, where tp is a global test. The global test tp consists of various elementary tests three tests [Kieffer et al., 2000] and they are robust to outliners as well as described in [Kieffer et al., 2000]. Also the purpose of the first two tests is to eliminate some infeasible configurations there by saving computational time. But if all the three tests are used when the range of the sensor is limited, it leads to scenarios in which some feasible configurations are ignored prematurely. Therefore in our case only two tests were used first test in-room test and the third test data test in [Kieffer et al., 2000]. The main consequence of not using the second test i.e. leg in test when the sensor range is limited is that it may increase the computation time. The interval robot position obtained using the ultrasonic sensors are then fused with the inertial sensors estimated robot position. In the case when the robot is moving the robots position needs to be tracked, in which case the robots position needs to predicted at the next instant to estimate the robots position at that instant. This is done by using the physical limitations of the robot based on the maximum possible speed and heading angle rate of the robot, instead of using a dynamic robot model with interval values [Jaulin et al., 2001] or position information from other sensors namely INS and encoders in this case. Thus we obtain an independent interval position of the robot from the ultrasonic sensors. 4. Interval Analysis based Adaptive mechanism for UKF. Sensor fusion is a very important and keenly researched topic in the domain of mobile robotics. For the problem of sensor fusion stochastic filters, such as Kalman filters, are commonly used. But they suffer from the same disadvantages described before i.e. an accurate model of the system and statistics are needed. In order to overcome these difficulties while using the UKF, a new approach has been introduced in this paper. As described above the robots position are estimated using two independent sources namely, the robot position from inertial sensors and the interval robot position from ultrasonic sensors. The position obtained using IA is updated only once every second due to computation time, where as the position from inertial sensors and encoders are updated 100 times per second. Moreover we know that the robots interval position to be guaranteed. The interval robot position thus obtained using ultrasonic sensors is like a plane or rectangle which basically means that there is equal probability with in that rectangle for the robot to be present anywhere inside that interval robot position. Then the estimated position using the INS is checked whether they are inside this rectangle. In case they are present inside the rectangle then they are trusted to be accurate and used. Alternatively if they are not then

5 Covariance ellipse enclosing interval robot poostion Inertial Sensors Encoders Ultrasonic sensors Map of robot environment UKF estimated robot position UKF Sensor fusion for estimating robot speed and orientation Interval Analysis algorithm robot position estimation for UKF estimated X and Y robot coordinates Interval X and Interval Y robot coordinates which forms a box or a plane Interval Robot Position Check whether the UKF robot position is inside the interval robot position box. If so the robot position is the UKF estimated X and Y coordinates. OR If not the robot position is the point which is closest to the UKF estimate in the interval box and the covariance is the ellipse that encloses the interval robot position. Figure 6: Fusion of UKF estimated robot position and covariance ellipse enclosing interval position Robot X and Y coordinate Sensor fusion block diagram Figure 5: Block diagram of IA based adaptive mechanism for UKF both the measurements need to be fused. But we know that the interval robot position is guaranteed to contain the robots position within that interval i.e a rectangle and also that there is equal probability for the robot to be present anywhere inside that rectangle i.e uniform distribution. So we know that the error in the robot position estimation is with the UKF estimated robot position only. Inorder to correct the UKF estimated robot positions we need to fuse them with the interval robot position. Thus there is a need to combine intervals with a real number from the UKF and also we need to know the covariance i.e a measure of uncertainty of the real number. One of the methods to obtain a real number and covariance from the intervals is to enclose the interval position by an ellipse which diagrammatically represents the covariance in 2D [Uhlmann, 1996] which the mid point of the intervals as the center of the ellipse. The major and minor axis radius of the ellipse are taken as the covariance matrix values. There are two options in selecting where the center of the ellipse is in the interval robot position. One of the options is to select the point on the rectangle box i.e. the interval robot position, which is geometrically closest to the robot position estimated using UKF with inertial sensors, thereby bounding the error in the UKF estimates. Also the interval uncertainty is converted into covariance for the UKF based on the error ellipse. But one of the disadvantages of using this method to fuse the interval data with the Kalman estimated data is that when the geometrically nearest point is chosen in the interval box, it introduces more uncertainty in the form of large covariance matrix values as one edge of the box will become the center of the ellipse. An alternate way of fusing the data is to choose the mid point of the interval positions and convert them in to covariance matrix values as described before and then fuse them with the UKF estimated position using the UKF as illustrated in the Figure 6. Figure 7: UKF only estimated robot position The algorithm described above has been successfully implemented in simulation using C++ and MATLAB software. Figure 7 shows the robot position estimate using UKF only, which is affected by sensor bias, drift etc. The Figure 8 shows the robot position after using the adaptive UKF robot position estimate using the SIVIA interval robot position for adaptation. Similarly the Figure 9 shows the UKF robot position estimation using the IMAGESP interval algorithm for the adaptive mechanism. In both cases it can be observed that the UKF estimate with the interval adaptive mechanism is much better than the UKF position estimate alone. It should be noted however that the SIVIA interval position uncertainty is greater than that of the IMAGESP algorithm. The reduction in uncertainty of the estimated interval robot position using the IMAGESP algorithm is attained at the cost of more computation time when compared with the SIVIA algorithm. This is because, in the IMAGESP algorithm, the whole initial subpaving is divided into many boxes of identical width less than or equal to ǫ 0.01 in our case and the global test tp is performed on all of these boxes. It can be seen in Figure 9 that when the position estimate of UKF is fused with the IA it results to very small position error. That is because of the small interval of the position estimate obtained using IMAGESP. Hence even if IMAGESP interval estimate comes at the cost of more computation time when compared with the SIVIA algorithm it results

6 the major and minor axis radius of the ellipse as the covariance matrix values. Simulation results show that the UKF robot position estimate with the IA based adaptive mechanism gives a more accurate estimate when compared to robot position estimates with UKF alone. References Alessandri, A., Bartolini, G., Pavanati, P., Punta, E., and Vinci, A An application of the extended kalman filter for integrated navigation in mobile robotics. American Control Conference. Figure 8: Fused robot position using SIVIA algorithm as adaptive mechanism in UKF Castellanos, J. and Tardos, J Mobile Robot Localisation and Map Building: A Multisensor Fusion Approach. Kluwer Ac. Pub., Boston. Clark, S., Dissanayake, G., Newman, P., and Durrant- Whyte, H A solution to simultaneous localisation and map building slam problem. IEEE Journal of Robotics and Automation, 173. Jaulin, L., Kieffer, M., Didrit, O., and Walter, E Applied Interval Analysis with examples in parameter and state estimation robust control and robotics. Springer-Verlag, London. Jaulin, L. and Walter, E Set inversion via interval analysis for nonlinear bounded-error estimation. Automatica, 294:1053 to Figure 9: Fused robot position using IMAGESP algorithm as adaptive mechanism in UKF in a much more accurate fused robot position estimate. Moreover, in Figure 8, it can be seen that the interval position uncertainty increases as expected when there are no land marks visible to the robots ultrasonic sensors. This can be seen in the Figure 8 when the robot begins to move and also at instances in between landmarks, when the number of ultrasonic sensors for which the landmarks are visible is less. 5. Conclusion An Unscented Kalman filter UKF using an Interval Analysis IA based adaptive mechanism has been described. Previous work on robot localisation and navigation using IA has been extended, so as to incorporate sensor range limitation. Moreover instead of using dynamic interval model of the robot to predict the next step of the robot, the physical limitations of the robot are used to predict the next step of the robot in the IA algorithm. A method for fusing the intervals with the UKF has been described, which is based on the concept of representing the intervals with an error ellipse in 2D and using Jetto, L., Longhi, S., and Venturini, G Development and experimental validation of an adaptive extended kalman filter for the localization of mobile. IEEE Transactions on Robotics and Automation, 152. Julier, S. and Uhlmann, J A new extension of the kalman filter to nonlinear systems. In Proc. of Aerosense: The 11thInt. Symp. on Aerospace/Sensing, Simulation and Controls. Kieffer, M., Jaulin, L., Didrit, O., and Walter, E Robust autonomous robot localization using interval analysis. Reliable Computing, 63:337 to 362. Kieffer, M., Jaulin, L., and Walter, E Guaranteed recursive nonlinear state estimation using interval analysis. Internal Report, Laboratoire des Signaux et Systemes. Uhlmann, J. K General data fusion for estimates with unknown cross covariances. In Proceedings of the SPIE Aerosense Conference. Wan, E. and der Merwe, R. V Kalman Filtering and Neural Networks, Chapter 7: The Unscented Kalman Filter. Wiley Publishing.