Transcription
SLAM for DummiesA Tutorial Approach to Simultaneous Localization and MappingBy the ‘dummies’Søren Riisgaard and Morten Rufus Blas1
1.Table of contents1.TABLE OF CONTENTS.22.INTRODUCTION .43.ABOUT SLAM .64.THE HARDWARE.7THE ROBOT .7THE RANGE MEASUREMENT DEVICE .85.THE SLAM PROCESS .106.LASER DATA .147.ODOMETRY DATA .158.LANDMARKS.169.LANDMARK EXTRACTION .19SPIKE LANDMARKS .19RANSAC.20MULTIPLE STRATEGIES.2410.DATA ASSOCIATION.2511.THE EKF .28OVERVIEW OF THE PROCESS .28THE MATRICES.29The system state: X .29The covariance matrix: P.30The Kalman gain: K.31The Jacobian of the measurement model: H .31The Jacobian of the prediction model: A .33The SLAM specific Jacobians: Jxr and Jz .34The process noise: Q and W.35The measurement noise: R and V .35STEP 1: UPDATE CURRENT STATE USING THE ODOMETRY DATA .36STEP 2: UPDATE STATE FROM RE-OBSERVED LANDMARKS .37STEP 3: ADD NEW LANDMARKS TO THE CURRENT STATE .3912.FINAL REMARKS .412
13.REFERENCES: .4214.APPENDIX A: COORDINATE CONVERSION.4315.APPENDIX B: SICK LMS 200 INTERFACE CODE.4416.APPENDIX C: ER1 INTERFACE CODE .5217.APPENDIX D: LANDMARK EXTRACTION CODE .823
2.IntroductionThe goal of this document is to give a tutorial introduction to the field of SLAM(Simultaneous Localization And Mapping) for mobile robots. There are numerouspapers on the subject but for someone new in the field it will require many hours ofresearch to understand many of the intricacies involved in implementing SLAM. Thehope is thus to present the subject in a clear and concise manner while keeping theprerequisites required to understand the document to a minimum. It should actuallybe possible to sit down and implement basic SLAM after having read this paper.SLAM can be implemented in many ways. First of all there is a huge amount ofdifferent hardware that can be used. Secondly SLAM is more like a concept than asingle algorithm. There are many steps involved in SLAM and these different stepscan be implemented using a number of different algorithms. In most cases we explaina single approach to these different steps but hint at other possible ways to do themfor the purpose of further reading.The motivation behind writing this paper is primarily to help ourselves understandSLAM better. One will always get a better knowledge of a subject by teaching it.Second of all most of the existing SLAM papers are very theoretic and primarilyfocus on innovations in small areas of SLAM, which of course is their purpose. Thepurpose of this paper is to be very practical and focus on a simple, basic SLAMalgorithm that can be used as a starting point to get to know SLAM better. For peoplewith some background knowledge in SLAM we here present a complete solution forSLAM using EKF (Extended Kalman Filter). By complete we do not mean perfect.What we mean is that we cover all the basic steps required to get an implementationup and running. It must also be noted that SLAM as such has not been completelysolved and there is still considerable research going on in the field.To make it easy to get started all code is provided, so it is basically just a matter ofdownloading it, compiling it, plugging in the hardware (SICK laser scanner, ER1robot) and executing the application; Plug-and-Play. We have used Microsoft Visual4
C# and the code will compile in the .Net Framework v. 1.1. Most of the code is verystraightforward and can be read almost as pseudo-code, so porting to other languagesor platforms should be easy.5
3.About SLAMThe term SLAM is as stated an acronym for Simultaneous Localization AndMapping. It was originally developed by Hugh Durrant-Whyte and John J. Leonard[7] based on earlier work by Smith, Self and Cheeseman [6]. Durrant-Whyte andLeonard originally termed it SMAL but it was later changed to give a better impact.SLAM is concerned with the problem of building a map of an unknown environmentby a mobile robot while at the same time navigating the environment using the map.SLAM consists of multiple parts; Landmark extraction, data association, stateestimation, state update and landmark update. There are many ways to solve each ofthe smaller parts. We will be showing examples for each part. This also means thatsome of the parts can be replaced by a new way of doing this. As an example we willsolve the landmark extraction problem in two different ways and comment on themethods. The idea is that you can use our implementation and extend it by using yourown novel approach to these algorithms. We have decided to focus on a mobile robotin an indoor environment. You may choose to change some of these algorithms sothat it can be for example used in a different environment.SLAM is applicable for both 2D and 3D motion. We will only be considering 2Dmotion.It is helpful if the reader is already familiar with the general concepts of SLAM on anintroductory level, e.g. through a university level course on the subject. There are lotsof great introductions to this field of research including [6][4]. Also it is helpful toknow a little about the Extended Kalman Filter (EKF); sources of introduction are[3][5]. Background information is always helpful as it will allow you to more easilyunderstand this tutorial but it is not strictly required to comprehend all of it.6
4.The HardwareThe hardware of the robot is quite important. To do SLAM there is the need for amobile robot and a range measurement device. The mobile robots we consider arewheeled indoor robots. This documents focus is mainly on software implementationof SLAM and does not explore robots with complicated motion models (models ofhow the robot moves) such as humanoid robots, autonomous underwater vehicles,autonomous planes, robots with weird wheel configurations etc.We here present some basic measurement devices commonly used for SLAM onmobile robots.The robotImportant parameters to consider are ease of use, odometry performance and price.The odometry performance measures how well the robot can estimate its ownposition, just from the rotation of the wheels. The robot should not have an error ofmore than 2 cm per meter moved and 2 per 45 degrees turned. Typical robot driversallow the robot to report its (x,y) position in some Cartesian coordinate system andalso to report the robots current bearing/heading.There is the choice to build the robot from scratch. This can be very time consuming,but also a learning experience. It is also possible to buy robots ready to use, like RealWorld Interface or the Evolution Robotics ER1 robot [10]. The RW1 is not soldanymore, though, but it is usually available in many computer science labs around theworld. The RW1 robot has notoriously bad odometry, though. This adds to theproblem of estimating the current position and makes SLAM considerably harder.ER1 is the one we are using. It is small and very cheap. It can be bought for only200USD for academic use and 300USD for private use. It comes with a camera and arobot control system. We have provided very basic drivers in the appendix and on thewebsite.7
The range measurement deviceThe range measurement device used is usually a laser scanner nowadays. They arevery precise, efficient and the output does not require much computation to process.On the downside they are also very expensive. A SICK scanner costs about5000USD. Problems with laser scanners are looking at certain surfaces includingglass, where they can give very bad readings (data output). Also laser scanners cannotbe used underwater since the water disrupts the light and the range is drasticallyreduced.Second there is the option of sonar. Sonar was used intensively some years ago. Theyare very cheap compared to laser scanners. Their measurements are not very goodcompared to laser scanners and they often give bad readings. Where laser scannershave a single straight line of measurement emitted from the scanner with a width of aslittle as 0.25 degrees a sonar can easily have beams up to 30 degrees in width.Underwater, though, they are the best choice and resemble the way dolphins navigate.The type used is often a Polaroid sonar. It was originally developed to measure thedistance when taking pictures in Polaroid cameras. Sonar has been successfully usedin [7].The third option is to use vision. Traditionally it has been very computationallyintensive to use vision and also error prone due to changes in light. Given a roomwithout light a vision system will most certainly not work. In the recent years, though,there have been some interesting advances within this field. Often the systems use astereo or triclops system to measure the distance. Using vision resembles the wayhumans look at the world and thus may be more intuitively appealing than laser orsonar. Also there is a lot more information in a picture compared to laser and sonarscans. This used to be the bottleneck, since all this data needed to be processed, butwith advances in algorithms and computation power this is becoming less of aproblem. Vision based range measurement has been successfully used in [8].We have chosen to use a laser range finder from SICK [9]. It is very widely used, it isnot dangerous to the eye and has nice properties for use in SLAM. The measurementerror is - 50mm, which seems like very much, but in practice the error was much8
smaller. The newest laser scanners from SICK have measurement errors down to - 5mm.9
5.The SLAM ProcessThe SLAM process consists of a number of steps. The goal of the process is to use theenvironment to update the position of the robot. Since the odometry of the robot(which gives the robots position) is often erroneous we cannot rely directly on theodometry. We can use laser scans of the environment to correct the position of therobot. This is accomplished by extracting features from the environment and reobserving when the robot moves around. An EKF (Extended Kalman Filter) is theheart of the SLAM process. It is responsible for updating where the robot thinks it isbased on these features. These features are commonly called landmarks and will beexplained along with the EKF in the next couple of chapters. The EKF keeps track ofan estimate of the uncertainty in the robots position and also the uncertainty in theselandmarks it has seen in the environment.An outline of the SLAM process is given below.Laser ScanLandmarkOdometry changeExtractionEKFDataOdometry updateAssociationEKFRe-observationEKFNew observationsFigure 1 Overview of the SLAM process10
When the odometry changes because the robot moves the uncertainty pertaining to therobots new position is updated in the EKF using Odometry update. Landmarks arethen extracted from the environment from the robots new position. The robot thenattempts to associate these landmarks to observations of landmarks it previously hasseen. Re-observed landmarks are then used to update the robots position in the EKF.Landmarks which have not previously been seen are added to the EKF as newobservations so they can be re-observed later. All these steps will be explained in thenext chapters in a very practical fashion relative to how our ER1 robot wasimplemented. It should be noted that at any point in these steps the EKF will have anestimate of the robots current position.The diagrams below will try to explain this process in more detail:Figure 2 The robot is represented by the triangle. The stars represent landmarks. The robotinitially measures using its sensors the location of the landmarks (sensor measurementsillustrated with lightning).11
Figure 3 The robot moves so it now thinks it is here. The distance moved is given by the robotsodometry.Figure 4 The robot once again measures the location of the landmarks using its sensors but findsout they don’t match with where the robot thinks they should be (given the robots location).Thus the robot is not where it thinks it is.12
Figure 5 As the robot believes more its sensors than its odometry it now uses the informationgained about where the landmarks actually are to determine where it is (the location the robotoriginally thought it was at is illustrated by the dashed triangle).Figure 6 In actual fact the robot is here. The sensors are not perfect so the robot will notprecisely know where it is. However this estimate is better than relying on odometry alone. Thedotted triangle represents where it thinks it is; the dashed triangle where odometry told it it was;and the last triangle where it actually is.13
6.Laser DataThe first step in the SLAM process is to obtain data about the surroundings of therobot. As we have chosen to use a laser scanner we get laser data. The SICK laserscanner we are using can output range measurements from an angle of 100 or 180 .It has a vertical resolution of 0.25 , 0.5 or 1.0 , meaning that the width of the areathe laser beams measure is 0.25 , 0.5 or 1.0 wide. A typical laser scanner outputwill look like this:2.98, 2.99, 3.00, 3.01, 3.00, 3.49, 3.50, ., 2.20, 8.17, 2.21The output from the laser scanner tells the ranges from right to left in terms of meters.If the laser scanner for some reason cannot tell the exact length for a specific angle itwill return a high value, we are using 8.1 as the threshold to tell if the value is anerror. Some laser scanners can be configured to have ranges longer than 8.1 meters.Lastly it should be noted that laser scanners are very fast. Using a serial port they canbe queried at around 11 Hz.The code to interface with the laser scanner can be seen in Appendix B: SICK LMS200 interface code.14
7.Odometry DataAn important aspect of SLAM is the odometry data. The goal of the odometry data isto provide an approximate position of the robot as measured by the movement of thewheels of the robot, to serve as the initial guess of where the robot might be in theEKF. Obtaining odometry data from an ER1 robot is quite easy using the built-intelnet server. One can just send a text string to the telnet server on a specific port andthe server will return the answer.The difficult part about the odometry data and the laser data is to get the timingright. The laser data at some time t will be outdated if the odometry data is retrievedlater. To make sure they are valid at the same time one can extrapolate the data. It iseasiest to extrapolate the odometry data since the controls are known. It can be reallyhard to predict how the laser scanner measurements will be. If one has control ofwhen the measurements are returned it is easiest to ask for both the laser scannervalues and the odometry data at the same time. The code to interface with the ER1robot is shown in15
Appendix C: ER1 interface code.16
8.LandmarksLandmarks are features which can easily be re-observed and distinguished from theenvironment. These are used by the robot to find out where it is (to localize itself).One way to imagine how this works for the robot is to picture yourself blindfolded. Ifyou move around blindfolded in a house you may reach out and touch objects or hugwalls so that you don’t get lost. Characteristic things such as that felt by touching adoorframe may help you in establishing an estimate of where you are. Sonars andlaser scanners are a robots feeling of touch.Below are examples of good landmarks from different environments:Figure 7 The statue of liberty is a good landmark as it is unique and can easily be seen fromvarious environments or locations such as on land, from the sea, and from the air.17
Figure 8 The wooden pillars at a dock may be good landmarks for an underwater vehicle.As you can see the type of landmarks a robot uses will often depend on theenvironment in which the robot is operating.Landmarks should be re-observable by allowing them for example to be viewed(detected) from different positions and thus from different angles.Landmarks should be unique enough so that they can be easily identified from onetime-step to another without mixing them up. In other words if you re-observe twolandmarks at a later point in time it should be easy to determine which of thelandmarks is which of the landmarks we have previously seen. If two landmarks arevery close to each other this may be hard.Landmarks you decide a robot should recognize should not be so few in theenvironment that the robot may have to spend extended time without enough visiblelandmarks as the robot may then get lost.If you decide on something being a landmark it should be stationary. Using a personas a landmark is as such a bad idea. The reason for this criterion is fairlystraightforward. If the landmark is not always in the same place how can the robotknow given this landmark in which place it is.The key points about suitable landmarks are as follows: Landmarks should be easily re-observable.Individual landmarks should be distinguishable from each other.Landmarks should be plentiful in the environment.Landmarks should be stationary.18
Figure 9 In an indoor environment such as that used by our robot there are many straight linesand well defined corners. These could all be used as landmarks.19
9.Landmark ExtractionOnce we have decided on what landmarks a robot should utilize we need to be able tosomehow reliable extract them from the robots sensory inputs.As mentioned in the introduction there are multiple ways to do landmark extractionand it depends largely on what types of landmarks are attempted extracted as well aswhat sensors are used.We will present basic landmark extraction algorithms using a laser scanner. They willuse two landmark extraction algorithm called Spikes and RANSAC.Spike landmarksThe spike landmark extraction uses extrema to find landmarks. They are identifiedby finding values in the range of a laser scan where two values differ by more than acertain amount, e.g. 0.5 meters. This will find big changes in the laser scan from e.g.when some of the laser scanner beams reflect from a wall and some of the laserscanner beams do not hit this wall, but are reflected from some things further behindthe wall.Figure 10: Spike landmarks. The red dots are table legs extracted as landmarks.The spikes can also be found by having three values next to each other, A, B and C.Subtracting B from A and B from C and adding the two numbers will yield a value.20
This method is better for finding spikes as it will find actual spikes and not justpermanent changes in range.Spike landmarks rely on the landscape changing a lot between two laser beams. Thismeans that the algorithm will fail in smooth environments.RANSACRANSAC (Random Sampling Consensus) is a method which can be used to extractlines from a laser scan. These lines can in turn be used as landmarks. In indoorenvironments straight lines are often observed by laser scans as these arecharacteristic of straight walls which usually are common.RANSAC finds these line landmarks by randomly taking a sample of the laserreadings and then using a least squares approximation to find the best fit line that runsthrough these readings. Once this is done RANSAC checks how many laser readingslie close to this best fit line. If the number is above some threshold we can safelyassume that we have seen a line (and thus seen a wall segment). This threshold iscalled the consensus.The below algorithm outlines the line landmark extraction process for a laser scannerwith a 180 field of view and one range measurement per degree. The algorithmassumes that the laser data readings are converted to a Cartesian coordinate system –see Appendix A. Initially all laser readings are assumed to be unassociated to anylines. In the algorithm we only sample laser data readings from unassociatedreadings.While there are still unassociated laser readings,and the number of readings is larger than the consensus,and we have done less than N trials.-Select a random laser data reading.do21
-Randomly sample S data readings within D degrees of this laserdata reading (for example, choose 5 sample readings that liewithin 10 degrees of the randomly selected laser data reading).-Using these S samples and the original reading calculate aleast squares best fit line.-Determine how many laser data readings lie within X centimetersof this best fit line.-If the number of laser data readings on the line is above someconsensus C do the following:o calculate new least squares best fit line based on allthe laser readings determined to lie on the old best fitline.oAdd this best fit line to the lines we have extracted.oRemove the number of readings lying on the line from thetotal set of unassociated readings.odThis algorithm can thus be tuned based on the following parameters:N – Max number of times to attempt to find lines.S – Number of samples to compute initial line.D – Degrees from initial reading to sample from.X – Max distance a reading may be from line to get associated to line.C – Number of points that must lie on a line for it to be taken as a line.The EKF implementation assumes that landmarks come in as a range and bearingfrom the robots position. One can easily translate a line into a fixed point by takinganother fixed point in the world coordinates and calculating the point on the lineclosest to this fixed point. Using the robots position and the position of this fixedpoint on the line it is trivial to calculate a range and bearing from this.Using simple trigonometry one can easily calculate this point. Here illustrated usingthe origin as a fixed point:22
Extracted lineExtractedlandmarkpointlandmarkLineorthogonal to(0,0)line landmarkFigure 11 Illustration of how to get an extracted line landmark as a point.23
Figure 12 The RANSAC algorithm finds the main lines in a laser scan. The green lines are thebest fit lines representing the landmarks. The red dots represent the landmarks approximated topoints. By changing the RANSAC parameters you could also extract the small wall segments.They are not considered very reliable landmarks so were not used. Lastly just above the robot isa person. RANSAC is robust against people in the laser scan.Another possibility is to expand the EKF implementation so it could handle linesinstead of just points. This however is complicated so is not dealt with in this tutorial.24
Multiple strategiesWe have presented two different approaches to landmark extraction. Both extractdifferent types of landmarks and are suitable for indoor environments. Spikeshowever is fairly simple and is not robust against environments with people. Thereason for this is that Spikes picks up people as spikes as they theoretically are goodlandmarks (they stand out from the environment).As RANSAC uses line extraction it will not pick up people as landmarks as they donot individually have the characteristic shape of a line.A third method which we will not explore is called scan-matching where you attemptmatch two successive laser scans. We name it here for people interested in otherapproaches.Code for landmark extraction algorithms can be found in Appendix D: Landmarkextraction code.25
10. Data AssociationThe problem of data association is that of matching observed landmarks fromdifferent (laser) scans with each other. We have also referred to this as re-observinglandmarks.To illustrate what is meant by this we will give an example:For us humans we may consider a chair a landmark. Let us say we are in a room andsee a specific chair. Now we leave the room and then at some later pointsubsequently return to the room. If we then see a chair in the room and say that it isthe same chair we previously saw then we have associated this chair to the old chair.This may seem simple but data association is hard to do well. Say the room had twochairs that looked practically identical. When we subsequently return to the room wemight not be able to distinguish accurately which of the chairs were which of thechairs we originally saw (as they all look the same). Our best bet is to say that theone to the left must be the one we previously saw to the left, and the one to the rightmust be the one we previously saw on the right.In practice the following problems can arise in data association:-You might not re-observe landmarks every time step.You might observe something as being a landmark but fail to ever see it again.You might wrongly associate a landmark to a previously seen landmark.As stated in the landmarks chapter it should be easy to re-observe landmarks. As suchthe first two cases above are not acceptable for a landmark. In other words they arebad landmarks. Even if you have a very good landmark extraction algorithm you mayrun into these so it is best to define a suitable data-association policy to minimize this.The last problem where you wrongly associate a landmark can be devastating as itmeans the robot will think it is somewhere different from where it actually is.26
We will now define a data-association policy that deals with these issues. We assumethat a database is set up to store landmarks we have previously seen. The database isusually initially empty. The first rule we set up is that we don’t actually consider alandmark worthwhile to be used in SLAM unless we have seen it N times. Thiseliminates the cases where we extract a bad landmark. The below-mentionedvalidation gate is explained further down in the text.1. When you get a new laser scan use landmark extraction toextract all visible landmarks.2. Associate each extracted landmark to the closest landmark wehave seen more than N times in the database.3. Pass each of these pairs of associations (extracted landmark,landmark in database) through a validation gate.a. If the pair passes the validation gate it must be thesame landmark we have re-observed so increment the numberof times we have seen it in the database.b. If the pair fails the validation gate add this landmarkas a new landmark in the database and set the number oftimes we have seen it to 1.This technique is called the nearest-neighbor approach as you associate a landmarkwith the nearest landmark in the database.The simplest way to calculate the nearest landmark is to calculate the Euclideandistance.1 Other methods include calculating the Mahalanobis distance which isbetter but more complicated. This was not used in our approach as RANSAClandmarks usually are far apart which makes using the Euclidean distance suitable.The validation gate uses the fact that our EKF implementation gives a bound on theuncertainty of an observation of a landmark. Thus we can determine if an observedlandmark is a landmark in the database by checking if the landmark lies within thearea of uncertainty. This area
SLAM for Dummies A Tutorial Approach to Simultaneous Localization and Mapping . There is the choice to build the robot from scratch. This can be very time consuming, but also a learning experience. It is also possible to buy robots ready to use, like Real .
