SIMULTANEOUS LOCALIZATION AND MAPPING WITH LIMITED SENSING USING EXTENDED KALMAN FILTER AND HOUGH TRANSFORM

Original scientific paper The problem of a robot to create a map of an unknown environment while correcting its own position based on the same map and sensor data is called Simultaneous Localization and Mapping problem. As the accuracy and precision of the sensors have an important role in this problem, most of the proposed systems include the usage of high cost laser range sensors, and relatively newer and cheaper RGB-D cameras. Laser range sensors are too expensive for some implementations, and RGB-D cameras bring high power, CPU or communication requirements to process data on-board or on a PC. In order to build a low-cost robot it is more appropriate to use low-cost sensors (like infrared and sonar). In this study it is aimed to create a map of an unknown environment using a low cost robot, Extended Kalman Filter and linear features like walls and furniture. A loop closing approach is also proposed here. Experiments are performed in Webots simulation environment.


Introduction
Usage of robots is very common in industry, space exploration projects, search & rescue operations, and in many other areas.Robot manipulators used in industry are usually fixed to ground and perform predetermined actions.However, robots used in space exploration projects and search & rescue missions are mobile and it is usually impossible to determine fixed actions.These robots are controlled by a human operator or they work autonomously.A mobile robot needs a model of the environment in order to perform its missions autonomously but it is not always possible to provide a model.In this situation, the robot has to create a model of the environment itself.While creating a model, the robot must navigate in the environment, get information using its sensors, save the information in a suitable format and go on navigating using its knowledge about the environment.During the exploration of the environment, uncertainty arises in the robot's position because there is always a margin of error between the expected action and performed action, odometry values are noisy and odometry sensors have low resolution.That is why a robot needs to create a model of the environment and localize using this model at the same time.This problem is called Simultaneous Localization and Mapping (SLAM) in literature.
As the accuracy, precision and range of the sensors have an important role in SLAM problem, high cost laser range sensors have been widely used in proposed systems.Since the introduction of Microsoft Kinect in 2010, there is also a high interest in using RGB-D cameras.Although there have been successful approaches using these sensors, there are real life scenarios that using these sensors are not appropriate.The robots built for house cleaning, the robots that are part of a swarm, disposable robots which will be used for hazardous material detection or mobile sensor networks must be low-cost.Laser range sensors are too expensive for these implementations.RGB-D sensors are relatively much cheaper but their sizes, power and CPU requirements make them unsuitable for some implementations.
In this study, a SLAM system using a low cost differential drive robot is presented.The robot has only 6 infrared sensors and gets its control signal from a controller PC via wireless communication.The method proposed here creates the map of an unknown environment using linear features like walls and furniture by Extended Kalman Filter (EKF) algorithm.Hough transform is used for the extraction of lines since other line extraction methods need denser and more accurate point measurements.Hough transform gives alternative lines with corresponding votes that helps handling measurements taken from two walls when turning a corner.Experiments are performed on Webots simulation environment [1].
There have been many studies on localization and mapping using linear features for similar purposes.
Einsele [2] deals with self-localization in an unknown environment in his work.He uses linear features extracted from laser scanner data to localize.He applies Iterative End Point Fit to data points for line segmentation.If a segment has fewer points than a threshold, it is discarded, else Least Square Line Fitting method is applied to the points that form the segment.A modified Dynamic Programming method is used for pattern matching.
Yun et al. [3] use linear features for robot localization.Hough transform is applied to sparse and noisy sonar sensor data and then Least Mean Square method is applied to the inlier points.Success of Hough transform for different parameter resolutions is considered in this work.
Großmann and Poli [4] apply Hough Transform to sparse and noisy sonar sensor data and make correction in robot pose by matching observed linear features with the ones in the map.Experiments are performed using a robot with seven sonar sensors.
Borges and Aldon [5] propose the Split-and-Merge Fuzzy method that is based on Prototype-based Fuzzy Clustering Algorithm but does not require the line number in advance.Borges and Aldon investigate two breakpoint detectors, Adaptive Breakpoint Detector and Kalman Filter Based Breakpoint Detector in another work [6].They also compare success of three line extraction methods; Line Tracking, Iterative End Point Fit, and Split-and-Merge Fuzzy.They show that Split-and-Merge Fuzzy, which is developed by them, is more successful.
Tardós et al. [7] use Hough Transform to extract lines and points.They propose a map joining technique that allows the system to build a sequence of independent limited-size stochastic maps and join them.First, the maps are joined using transformation.Then Joint Compatibility Branch and Bound Algorithm that is developed by Neira and Tardós [8] is applied to find features in different local maps that correspond to the same environment feature.
Burguera et al. [9] apply Iterative Closest Point method, which is a scan matching algorithm that is usually applied to laser scanner data, to sonar sensor data.They add a pre-process step to group sensor readings and a post-process step to make correction on robot trajectory that is involved in data collection and grouping.They compare Iterative Closest Point method to Iterative Dual Correspondence method and find that the latter one gives better results.
Garulli et al. [10] use EKF for mapping using linear features.They apply segmentation and line fitting steps to laser scanner data one after another continuously until a stopping condition is reached.In segmentation step they use the conditions that exist in Successive Edge Following and Line Tracking: If the distance between the current point and the segment is less than a threshold and the distance between the current point and the end point of the segment is less than a threshold, the point is added to the segment, else a new segment is started.Segments that are shorter than a threshold or contain points less than a threshold are discarded.Covariance matrices are obtained by statistical properties of data.While comparing observed lines with the stored ones, angle difference, distance and overlap ratio between the lines are taken into account.
Beevers and Huang [11] aim to find a solution to SLAM problem using a robot with limited sensors, Rao-Blackwellized Particle Filter and linear features.For feature extraction, they first use a method similar to Adaptive Breakpoint Detector to group sensor data.After this, they apply Iterative End Point Fit for better segmentation and then Least Squares Estimator to get line parameters.
Choi et al. [12] use EKF with linear features for mapping.They assume that all the lines are parallel or orthogonal to each other.In order to extract linear features from sparse and noisy sensor data they use Pseudo Dense Scan, Resampling, Iterative End Point Fit, Least Square Line Fitting, and Constrained Hough Transform methods.For data association they use Euclidean distances between parameters of lines in Hough space.They use a robot with seven infrared sensors in experiments.
Yap and Shelton [13] use a robot that has sonar sensors and Particle Filter for line based mapping.They assume that lines in the environment are orthogonal or parallel to each other.They use Randomized Hough Transform to find collinear points, after this they use Successive Edge Following to find breakpoints for segmentation.In order to compare the new observed lines with the ones stored before, they consider the perpendicular distance between them and the distance between their end points.
An et al. [14], use line segments as features and Rao-Blackwellized particle filter for SLAM.They extract line segments from laser scanner data using Iterative End Point Fit method.
Stavrou and Panayiotou [15] deal with localization problem of a robot with a single infrared sensor.They use Monte Carlo Localization and perform their experiments on a small map that is two times bigger than the maximum range of the sensor.
In [16], Thomas and Teresa use EKF based SLAM.In this study, linear features are extracted from laser scanner data using RANSAC method.
In [17], EKF is used for GPS based localization while mapping the environment, and genetic algorithm is used to merge occupancy grid maps of vehicles.
In [18], EKF is used in sensor fusion of radar, ultrasonic and odometry data for the localization of a mobile robot.

Proposed method
In this study, it is aimed to perform simultaneous localization and mapping using a wheeled robot with limited sensors.

Robot model
The robot model used in this study, shown in Fig. 1, is a differential wheeled robot, which has 6 infrared sensors and gets control signals from a controller PC via wireless communication.

Feature extraction
In this study, we use linear features like walls and furniture.As we use sparse sensors, it is difficult to extract lines from a single measurement.In order to overcome sparseness a common method which consists of collecting sensor measurements and having a pseudo dense scan is used [11,12].In this method while the robot is moving, sensor measurements are taken, if an object is seen then a buffer of a certain size is filled with point data.
Hough Transform [19] is the main line extraction method in our implementation.In Hough Transform, while choosing between line candidates, we used all the (, ) parameters that are voted more than a threshold because the data collection may contain more than one line as the robot may see more than a single wall while collecting data points.
We used two different measurement buffers for right and left sides, because they do not have any chance to get points from the same wall.Front sensors are only used to avoid collisions.
In this work, lines are all represented by (, ) parameters in the system.End points of lines are also stored separately.These end points are used in map drawing step, they are also used in landmark comparison which will be explained in the next section.

Exploration
In this study, we use frontier-based exploration [20,21].In frontier-based exploration, robot is moved towards the cells on the boundary between explored open area and unexplored area, which are called frontier cells.In our study, we store an occupancy grid map with low resolution, and perform Dijkstra's algorithm [22] on the map to find the next exploration target with least cost.The occupancy values in each cell of the map are updated with sensor measurements, if an object is seen, the value of the cell is decreased, and if nothing is seen, the value is increased.This helps us compensate uncertainty in sensor readings.

EKF based SLAM
Extended Kalman Filter (EKF) based SLAM algorithm is probably the earliest SLAM algorithm [23].EKF was introduced to the SLAM problem by Smith and Cheeseman [24] and first implementation was the work by Moutarlier and Chatila [25,26].In EKF, measurement noise and motion noise are represented by Gaussian distributions; mean values of robot poses and landmarks are stored in a column matrix and covariance values are stored in a square matrix.
As we have mentioned before, robot pose is represented by (, ,   ) parameters and a landmark is represented by (, ) parameters.Mean values for these parameters are arranged in the mean vector as given in Eq. (1).
The arrangement of the variance values of robot and landmarks in covariance matrix can be seen in Eq. (2).
In EKF based SLAM algorithm, first the action update procedure works; using kinematic equations and odometry values, mean vector is updated.Covariance matrix is also updated; motion noise covariance is added to the covariance matrix.
After the action update step, measurement update step begins.In measurement update step, observed landmarks are compared to the ones that are seen and stored before.If the observed landmark is associated to a stored landmark, correction on robot and landmark positions is made depending on measurement difference and uncertainty.
Although landmarks are stored using their (, ) parameters in the global reference frame, in comparison process their poses relative to the robot are used Eq. ( 3).
In comparison process, the similarity is measured using Mahalanobis distance between landmarks (4).
In Eq. ( 4),    is the th landmark that is observed, ̂  is the th landmark that was seen before and stored.Ψ  is the total covariance of new observation and stored landmark. is the time index.
In our implementation we do not only use Mahalanobis distance to understand if an observation and a previously stored landmark correspond to the same environment feature.There are some situations that eliminate the possibility of these lines to correspond to the same line.These situations are listed below: -If the observation is far away from the end points of the stored landmark.-If the landmark is in a position that robot cannot see using the sensors that made the observation.-If the landmark is not in the sensor range.
-If the angle difference and distance between the landmark and observed line are greater than corresponding thresholds.
While comparing landmarks, checking end points is important because if they are not checked two different lines with similar parameters can be accepted as the same line, as it can be seen in Fig. 2.
Figure 2 Two walls that can be perceived as the same wall as they are represented by the same line parameters

Loop closing problem
Loop closing is the problem of recognizing when the robot has returned to a place it has already been in cyclic environments.
In our work, we use stored end points of landmarks in order to discriminate whether two landmarks with same line parameters correspond to the same wall or they are two different walls.This approach leads to problems when robot returns to its starting position after a tour in a cyclic environment (Fig. 3).In Fig. 3, the starting point of the robot is marked with a filled black rectangle and its direction is also shown.When the mapping process starts, robot sees the wall on its left (landmark 1), and stores it as it is a new landmark.After a tour in the environment, robot sees another wall (landmark 2) that has similar parameters to the wall it saw in the beginning.Because of the distance between end points, this wall is handled as a new landmark, which is possible.After robot moves forward and returns to its starting position, it is seen that two landmarks actually indicate the same wall.As it is mentioned before, in order to associate stored landmarks and observations, the landmark with minimum Mahalanobis distance to the observation is found.
In loop closing problem, it is aimed to find two similar landmarks and merge them.But checking all the landmark pairs is time consuming.Instead of this, while associating landmarks and observations in each step, not only one, but two landmarks with minimum Mahalanobis distances to the observation are found (these walls are numbered in Fig. 3).As the parameters of the last added landmark (landmark 2) are found according to the recent belief, this landmark gives the minimum distance to the observation.If the robot has come to a loop closing point then the landmark that was added in the beginning (landmark 1), gives the second minimum distance.After two landmarks that are most similar to the observation are found, these two landmarks are compared using their end points and the Mahalanobis distance between them.If these landmarks are close enough to each other, it is understood that these two landmarks indicate the same wall.At this point, we do not merge these two landmarks, because merging does not make any correction on previously seen landmarks.Instead of merging we use the more reliable one to update the whole state: The observation is discarded, landmark 1 is used as an observation and landmark 2 is accepted as its associated landmark.The reason for choosing landmark 1 as an observation and using it as a correction factor results from the fact that this landmark has less uncertainty as it is seen in the beginning of the mapping process when the robot had little uncertainty about its position.

Experimental setup and results
Experiments are performed using Webots Simulation Software [1] developed by Cyberbotics Company.To test the performance of the proposed method three environments with different properties are implemented.
The first environment (Fig. 4) is 7 × 7 meters square, that is designed to be symmetrical and mainly to test the algorithm's ability to correctly localize the robot(s).During the tests different sensor noise levels are used.As our exploration strategy allows, along with the single robot experiments, we also made experiments using two robots.The second environment (Fig. 5) is 9 × 5,3 meters rectangle, containing perpendicular walls and spaces between the same levelled walls, especially aiming to test the ability of the algorithm to reflect such structures on the map.The third and final environment (Fig. 6) is sized 7 × 5,5 meters and contains both perpendicular and angular corners between the walls.Rectangular and somehow amorphous obstacles are also placed in the environment creating an unshaped corridor for the robot(s).

Results with single robot
A single robot with no sensor noise and added different sensor noise levels are used during the first part of the experiments.Visual results given in this section are chosen arbitrarily from 10 runs.
For each of the tests first no noise is introduced to the sensors.Then noises with 2 cm and 4 cm standard deviation are added respectively.Fig. 7 shows the results for the first environment.As it can be seen from Fig. 7a, while there is no added noise on sensor measurements the map is created fairly correct except that there are small missing parts of the walls.This is principally caused by the low frequency of the sensor observations; if no measurement has been taken for a predetermined distance, the algorithm will end a wall and assume the next measurement as the starting point of the wall.
Depending on the amount of sensor noise some artificial obstacles arise on the map, as the ones marked with circles in Fig. 7b and 7c.These result from noisy sensor readings and adding the fitted lines as new landmarks.
Fig. 8 shows the results for the second environment.Again, while there is no added noise on sensor measurements the map is created successfully as shown in Fig. 8a.Additionally, the door marked with rectangle is identified correctly and no serious correspondence problem occurred.Data association problems that occurred after adding noise to the sensors is marked with circles in Fig. 8b and 8c.Also it is observed that in Fig. 8b the door does not exist anymore, which is caused by misidentification of the distance between walls.Fig. 9 shows the results for the third environment.As shown in Fig. 9a, the map is created successfully and the slope of the walls is identified correctly while there is no added noise on sensor measurements.Also parallel edges of the close obstacles did not create problem in discerning.When sensor noise is added, few artificial obstacles appear in the map (Fig. 9c).

Results with two robots
As our exploration strategy allows, similar experiments are repeated by using two robots.Initial positions of these robots are known and it is assumed that they are both controlled by a central controller; they send sensor measurements to the controller and get action signals.
Two of the challenges of using multiple robots is discerning other robots from the obstacles within the environment and having more data to associate.Although having more data may become useful, they can also increase the correspondence problems if they embrace too much noise.
Visual results given in this section are again chosen arbitrarily from 10 runs.Fig. 10 shows the results for the first environment.As in Fig. 10a, while there is no added noise on sensor measurements the map is created fairly correct.Moreover having two robots consequently more sensor measurements helped the algorithm to overcome some noise as shown in Fig. 10b and Fig. 10c.
Once more, compared to the results obtained with a single robot, the method was able to identify the narrow door in second environment by using two robots, marked with rectangles in Fig. 11.The correspondence problem is solved more successfully with two robots as in Fig. 11a and becomes more severe only for increasing noise levels as marked with circles in Fig. 11c.
For the third environment, using two robots improved the map as shown in Fig. 12, in terms of correspondence problem, especially when a noise parameter is introduced.

Discussion
A system to create a map of an environment using a robot with limited sensing is proposed.Even though the line extraction method causes some erroneous landmarks, it is shown that the proposed landmark representation and exploration strategy are good solutions for this problem.
The exploration strategy proposed here also allows using multiple robots which decreases the exploration time and increases the rate of correct data associations.
Usually the estimated state for robotic application is robot's pose information.Therefore it is very important to get the robot pose correct for exploration, navigation or mapping tasks.In this study, it is observed that EKF improves robot's pose information significantly allowing mapping algorithm to perform better.
The pose of the differential drive robot used here corresponds to an x-y position (x, y) and an angular orientation (θ).To examine the improvement achieved by using EKF on estimation of these parameters, errors are measured against ground truth when robot travels around a square environment four times.Figs.13a and 14a show the errors in x, y position parameters as meters while EKF is not used.When EKF is used, x, y position estimation becomes more accurate and stabilized as shown in Fig. 13b and 14b.Error rates occurring in angular orientation θ by not using EKF as shown in Fig. 15a are also improved and stabilized as in Fig. 15b.
Considering all pose parameter errors are incremental, meaning they increase as the travelled distance increases, the overall contribution of EKF will be greater as the travelled distance increases.

Conclusion
In this study it is aimed to explore and create a map of an unknown environment using a mobile robot that has 6 infrared sensors using EKF based SLAM algorithm and linear features like walls.For this purpose, Hough Transform is used for line extraction from data points that are collected by sensors.A solution to loop closing problem is proposed, which finds two landmarks that are most similar to the observation, checks the Mahalanobis distance between these landmarks, and if conditions are satisfied discards the observation and makes corrections according to the previously added landmark.As these two landmarks are not just merged but one of them is handled as an observation, some correction can be made in not only the matching two landmarks but in the entire state vector (which is a property of EKF).In the experiments it is seen that this solution increases the success.Experiments with two robots have been held which halved the exploration time and made some improvements in the maps.In the future we will study on improving line extraction method as it causes some faulty landmarks when noise increases.

Figure 1
Figure 1 Robot model Robot pose is represented by three parameters, (, ,   ), where (, ) represents the coordinates of the centre of the robot and   represents heading angle.

Figure 3
Figure 3 Loop closing a) Two landmarks that are discovered to be associated in loop closing process b) The same wall perceived as two distinct landmarks when loop closing is not used c) Position of landmarks after the correction made in loop closing

Figure 7 Figure 8 Figure 9
Figure 7 Maps of the first environment created by single robot a) No noise b) Sensor noise with 2 cm standard deviation c) Sensor noise with 4 cm standard deviation

Figure 10 Figure 11 Figure 12
Figure 10 Maps of the first environment created by two robots a) No noise b) Sensor noise with 2 cm standard deviation c) Sensor noise with 4 cm standard deviation

Figure 13 Figure 14 Figure 15
Figure 13 Error in xa) when EKF is not used b) when EKF is used