Implementation of Odometry with EKF for Localization of Hector SLAM Method

Transcription

1 Implementation of Odometry with EKF for Localization of Hector SLAM Method Kao-Shing Hwang 1 Wei-Cheng Jiang 2 Zuo-Syuan Wang 3 Department of Electrical Engineering, National Sun Yat-sen University, Kaohsiung, Taiwan hwang@ccu.edu.tw 1 enjoysea0605@gmail.com 2 t1ina2003@gmail.com 3 Abstract For large-scale indoor laser scan mapping, a challenging problem is scan information lack of distinguishable landmarks, thus cause the improper mapping result especially in corridor environment. Collision may happen during the autonomous robot navigation. This paper presents a scan matching algorithm with odometer prediction using Extended Kalman Filter (EKF) and an optimal path planning with regression subgoal. The scan matching process will be able to out of local minima, and has an effective correction in the odometry information. By iterating odometer correction in each step, the matching result will be much better than one only believe in scan or odometry. Both simulated and large-scale indoor experimental data under the same conditions are used to verify the effectiveness of the proposed techniques. Keywords SLAM; Autonomous Robot; Extended Kalman Filter; Path Planning; Regression. I. INTRODUCTION In today s world, robots are getting more important in human's life, not just robot arms for the manufacturing propose. The use of autonomous robots has been applied to industry and academic applications and many other fields. The low cost, size and availability of sensors and micro-processor made the autonomous robots affordable for office and home. Indoor navigation technique have experienced an amount of interest for research proposes over the last three decades. Recent research are working on utilizing the existing algorithm to find an effective way for autonomous robots that will make robots into our society without harm and people can work with robots, live with robots. The robot navigation in dynamical environment can be divided into two parts. The first part is how to create accurate map of the environmental characteristics and the second parts is to generate a safe path with the area-correct map and dynamic obstacle. Many researches about SLAM and navigation algorithms has been developed to solve common scenarios in the environment [1][2]. Furthermore, the methods should guarantee their performance with cheap sensors and can be easily obtain on the navigation program [3]. Several researcher of SLAM have been done in recent years, typical indoor environment using Rao-Blackwellized particle filters (RBPFs) will be a highperformance solutions, and has be write into open source software as Gmapping [4]. Another popular implementations is Bayes filters which iteratively calculate posterior distributions of the robot poses with landmarks, and majority of this method come with Kalman Filters (KFs) [5]. Among these two decade, there are wealth of research on KF-based algorithm have been implemented in the literature, such as the EKF and RBPFs. The main objective of this paper is to create a navigation scheme for differential wheeled robot that is accurate in positioning with regard to the original point of the world. The kinematic scheme keeps take a slip within predicting posture procedure at all times. It is hard to estimate or measure for all kinds of differential wheeled robots static coefficient of slip at a constant velocity, and fusion of the control signal and odometry measurement using extended kalman filter (EKF) can predict odometry information by looking at what command comes in. An odometry EKF should be used to compensate online to ensure the route of the differential wheeled robot reasonable from the start point. Proposed method assume odometry can be trusted in short moving distance, and has no any instantaneous large-angle movement. There has been a plenty of research in path planning, with working solutions for common office indoor scenarios using best-first search like A* algorithm, it's easy to generate path from start point to end point. The only concern is, if the robot receive too many pose in the simple route and perform unnecessary pose in order to reach a specific point, will slow down whole navigation speed. Reduction on the poses of path is achieve by using linear regression on each pair of poses and compare each line to collect poses of shortest path. Basically this method will filter the paths with an angle, remain the key pose in the path, such as corner, U-turn, two point in straight line. II. BACKGROUND A. Hector SLAM Hector SLAM algorithm is selected for this research. This is primarily because this SLAM algorithms is suited to the condition that odometer information cannot be acquired, or error of the odometer is over the tolerance. High scanning rates long-range rangefinder is required in this method. Given a point in a continuous map, P, the occupancy value, M( ), the gradient will be in the form:. M(P ) =( ( ), ( ) ) (1)

2 If approximated by using the bilinear method mention before, linear interpolation with closest integer coordinates P,P,P,P can be present as: M(P ) ( ) + ( ) + ( ) + ( ) And derivatives of the map in specific point can be show as: M(P ) M(P ) ( ) +( ) + ( ) +( ) ( ) +( ) + ( ) +( ) Begin with a start estimate pose. ξ=,,. The target function aim to minimize the error of the occupancy of end-point () and map (value 1, means the obstacle exist in the map.), can be written as follows, = 1 () where () is the transform of the end-point scan received in robot frame to the world frame, () = () () (2) (3) (4) () (), +, (5) Gauss Newton algorithm is used to solve non-linear least squares problems, target function can be defined as: =1 () (6) The recurrence relation for Newton's method for minimizing a target function: = (7) The gradient vector of target function, assume robot position has a very little movement Δ, small enough to ignore: = = () () 1 () (8) And H denotes the Hessian matrix, obtained by ignoring the second-order derivative terms H= = () () () () where derivative of () can be represent by matrix: () = 1 0 (), cos(), 0 1 cos(), (), (10) And now, calculate a step Δ towards the minimum, Δ = () () 1 () (9) (11) The non-smooth linear approximations method in Hector SLAM with a point coordinate in the map relies on the scan matching information at each end-point for proper convergence and suffers from strong local minima in the corridor environment. B. Extended Kalman Filter The Markov localization model represents robot position using probability density function, which is very general but lack of efficiency. In Kalman filter, next state probability (Motion Model) (, ) must be a linear function with Gaussian noise, and can be expressed by following equation: = + + (12) Here and are state vectors, and is control command at time. is a matrix imply that how the state evaluate from previous state without controls or noise. is a matrix of corresponding control changes map to the next state. is the random variables represent prediction noise, zero mean Gaussian noise. Measurement probability (Observation Model) (, ) as following: = + (13) where is a matrix of relation of observation and state. When robot in the state, the observation robot will received ideally. Extended Kalman Filter (EKF) can overcome the assumption Kalman Filter (KF) that motion model has linearity. The key idea of EKF is called linearization. Assumption yield next state probability and measurement probability are governed by nonlinear functions and h, x =(, ) + (14) z =h( ) + (15)

3 EKF calculates an approximation to the true belief, which is represented by a Gaussian. The belief state ( ) at time is represented by a mean and a covariance Σ. III. PROPOSED METHOD System overview of the proposed scheme is illustrated in Fig. 1. The proposed scheme can be divided into two parts, one is the map construction in the SLAM, and the other is path poses reduction in the navigation process. Main contribution of this paper in the SLAM is fusion of the odometer and control command that the output is the initial estimate pose, input command and odometer reading information is fused in the EKF in SLAM process when robot receives a desired input. SLAM Navigaiton Odometer Estimate Pose EKF Scan Matching Path Planning Mapping Map SubGoal LIDAR Controller Fig.1. System overview of the proposed scheme A. Odometer EKF with Scan Matching Original scan matching method proposed in Hector SLAM is not able to jump out of local minima in the long distance corridor environment, it only performs a good result with the high-end laser rangefinder device. The architecture of the combined algorithm is shown in Fig 2. Encoder Position Presiction position estimate Predicted observations Estimation (fusion) Scan Matching Observation Matched predictions and acutal observation raw scan data Fig.2. Architecture of combined EKF and scan matching Method proposed is a modification in the odometer sensor reading, by fusing raw sensor readings fusion with control command u = (, ). The EKF motion model as equation 14, a nonlinear motion equation as follows: cos( +) (ξ, ) =sin( +) ξ (17) 1 The odometer sensor readings is z = (, ), then calculate in the EKF =Σ ( Σ + ) (18) And the next state of the odometer can be found by the equation = + (19) Refresh the new variance matrix: Σ = ( )Σ (20) And submit the result into scan matching process. Transform the end-point into the world coordinate: ( ) = ( ) ( ) ( ) ( ), + (21), The gradient vector of the target function will be = ( ) ( ) And Hessian matrix = ( ) ( ) ( ) Finally add the step pose Δ to : 1 ( ) ( ) (22) (23) = + H (24) Algorithm will draw the scan lines with regard to pose at that moment into the map when the scan matching ended. The first part is use the odometer readings and control command in the EKF. B. Path Planning with Subgoal A* path search algorithm generate an optimal path in the environment with obstacle cost. Each node has a state value of sum of total cost and heuristic function value h, and next neighbor state value will accumulate the current state value into the total cost value. Equation is shown as follows: g =g( ) +cost, (25) The cost function is the distance between the current state and the neighbor state. h(h) = (, ) (26) The heuristic function value h is the distance between current (, ). In the whole process, make sure pick the lowest state value (or lower heuristic value when the

4 same), the shortest path will be found if it exists. The subgoal node takes the path ={, } pass from the A* algorithm as the input, and uses the first and second poses in the path as the standard regression line, definition is shown below: =, (27) And measure whether the next two point line is in the tolerance angle range.,, h h (28) Assume the empty ouput set = {}, if angle is larger than the previous regression line, add the current point into the output set, Q (29) After examining all the node in the path, the set will remain the refined poses. IV. EXPERIMENT In this section, implementation of the proposed method on the U-bot robot platform is described. The experimental setup and the flow of information is outlined in section A. The comparison of experiment results of the Hector, odometry update and odometry EKF are conduct in section B. A. Environment Setup The environment is a corridor field with a 6 foyer in the middle of the map. The experiment was carried out using U-bot, a differential wheeled autonomous robot as the motion platform. The 4m short distance laser range finder (URG-04LX) mounted on U-bot platform is used to acquire the ground truth of the environment. The computation platform (Laptop) has an Intel Core i5-5200u, 4GB memory, GT 930M graphic card. Fig. 3 shows the much better map based on the 30m long distance laser rangefinder UTM-30LX, and the result is perfect in distance translation and angular transformation. Fig. 4 depicts the feature points in the environment, short corridor, long corridor and foyer. Long corridor has almost the same scan information along the way, and has a long distance path which can determine the straight line characteristic of the SLAM. Fig.4. Feature point in the map B. Experiment Result In this paper, we performed an experiment in environment described in section A to verify the effectiveness of the proposed approach. Hector SLAM localization algorithm, odometry update, and odometry EKF were performed for comparison by the means of record file. As shown in Fig. 5, robot using the hector SLAM method draw the map with incorrect distance in corridor environment. Laser scan cannot identify the difference of walls in the route, and lack of distinguishable landmark in the foyer area, results in wrong mapping in the length and angular distance. Fig. 6 shows that the improved scan matching method with odometer update which believes in odometer in short distance movement and applies it to the current position, and map generated with satisfied distance in corridor environment. Overshot in the position estimate occurs when scan matching calculation fell in the same direction of the odometer updating, being too much optimistic of the non-local-minima area. Fig.5. HectorSLAM mapping result Fig.3. Map generate with 30m Laser rangefinder

5 Fig.6. Odometer update and scan matching Fig. 7 illustrate the map built by scan matching with odometer in extended Kalman filter, consequence of proposed method was not too much optimistic and not too much pessimistic with regard to the current position, and value of length is between the HectorSLAM and odometer updating. Three trajectory are compared in the Fig. 8, and the trajectory of the proposed method is the fusion of the rest two method trajectory. This paper provide a novel method of SLAM localization problem in the corridor environment with a low-cost laser rangefinder. In path planning, a redundant pose reduction method which increase efficiency of navigation when robot is following the path generate by a* algorithm. This paper utilize ROS as the robot message platform, and makes some technique can be accessed by searching for exist package repository in the ROS wiki. From what has been mentioned above, the robotic SLAM localization using EKF is effective in any place include corridor scenario. In SLAM, the decisions for next-step mapping are made based on the result of previous map constructed by robot. Other work add new constraints in the existing nodes of map if possible, instead of crafting new nodes to the map [6]. In additional, higher quality map has been obtained by fusing volumetric surface in map reconstruction [7]. Update the constructed map using place recognition and subsequent loop closure constraints follow the rule of as-rigidas-possible space deformation. The alternative device for lowcost autonomous robot navigation is webcam or CCD camera, which has been studied for decades and has been proven in performance of the stability of mapping. In the real world, using the feature in the camera image and calculate the point movement in the two image frame to localize the robot s orientation in the map. Lim et al. proposed a SLAM system with monocular device that uses a relative coordinate system, local bundle adjustment can be performed by the system on a small subset of key frames [8]. Fig.7. OdometerEKF and scan matching Fig.8. Trajectory of three comapred method V. CONCLUSION REFERENCES [1] J. E. Guivant and E. M. Nebot, Optimization of the Simultaneous Localization and Map-Building Algorithms for Real-Time Implementation, in IEEE Trans. on Robotics and Automation, vol. 17, no. 3, pp , [2] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges, in Proc. of the Sixteenth Int. Joint Conf. on Artificial Intelligence, pp , [3] I. Ashokaraj, P. Silson, and A. Tsourdos. Application of an extended Kalman flter to multiple low cost navigation sensors in wheeled mobile robots. Sensors, 2: , [4] G. Grisetti, C. Stachniss, and W. Burgard, Improved techniques for grid mapping with rao-blackwellized particle filters, IEEE Transactions on Robotics, vol. 23, no. 1, pp , [5] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, FastSLAM: A factored solution to the simultaneous localization and mapping problem, in Proc. AAAI Nat. Conf. Artif. Intell., 2002, pp [6] H. Johannsson, M. Kaess, M. Fallon, and J. J. Leonard, Temporally scalable visual SLAM using a reduced pose graph, in RSS Workshop on Long-term Operation of Autonomous Robotic Systems in Changing Environments, [7] T. Whelan, M. Kaess, H. Johannsson, M. Fallon, J. J. Leonard, and J. McDonald. Real-time large-scale dense RGB-D SLAM with volumetric fusion. Robotics Research, December [8] H. Lim, J. Lim, and H. J. Kim, Real-Time 6-DOF Monocular Visual SLAM in a Large-Scale Environment, in International Conference on Robotics and Automation (ICRA), 2014, pp