Slam Tutorial

Embed Size (px)

DESCRIPTION

A very useful tutorial to know about SLAM for beginners.

Text of Slam Tutorial

SLAM for DummiesA Tutorial Approach to Simultaneous Localization and Mapping

By the dummies

Sren Riisgaard and Morten Rufus Blas

1

1.1. 2. 3. 4.

Table of contentsTABLE OF CONTENTS.........................................................................................................2 INTRODUCTION ...................................................................................................................4 ABOUT SLAM ........................................................................................................................6 THE HARDWARE..................................................................................................................7

THE ROBOT .....................................................................................................................................7 THE RANGE MEASUREMENT DEVICE .................................................................................................8 5. 6. 7. 8. 9. THE SLAM PROCESS .........................................................................................................10 LASER DATA .......................................................................................................................14 ODOMETRY DATA .............................................................................................................15 LANDMARKS.......................................................................................................................16 LANDMARK EXTRACTION ..............................................................................................19 SPIKE LANDMARKS .......................................................................................................................19 RANSAC.....................................................................................................................................20 MULTIPLE STRATEGIES..................................................................................................................24 10. 11. DATA ASSOCIATION.....................................................................................................25 THE EKF ..........................................................................................................................28

OVERVIEW OF THE PROCESS ..........................................................................................................28 THE MATRICES..............................................................................................................................29 The system state: X ..................................................................................................................29 The covariance matrix: P.........................................................................................................30 The Kalman gain: K.................................................................................................................31 The Jacobian of the measurement model: H .............................................................................31 The Jacobian of the prediction model: A ..................................................................................33 The SLAM specific Jacobians: Jxr and Jz ..................................................................................34 The process noise: Q and W.....................................................................................................35 The measurement noise: R and V .............................................................................................35 STEP 1: UPDATE CURRENT STATE USING THE ODOMETRY DATA .......................................................36 STEP 2: UPDATE STATE FROM RE-OBSERVED LANDMARKS ..............................................................37 STEP 3: ADD NEW LANDMARKS TO THE CURRENT STATE .................................................................39 12. FINAL REMARKS ...........................................................................................................41

2

13. 14. 15. 16. 17.

REFERENCES: ................................................................................................................42 APPENDIX A: COORDINATE CONVERSION.............................................................43 APPENDIX B: SICK LMS 200 INTERFACE CODE......................................................44 APPENDIX C: ER1 INTERFACE CODE .......................................................................52 APPENDIX D: LANDMARK EXTRACTION CODE ....................................................82

3

2.

Introduction

The goal of this document is to give a tutorial introduction to the field of SLAM (Simultaneous Localization And Mapping) for mobile robots. There are numerous papers on the subject but for someone new in the field it will require many hours of research to understand many of the intricacies involved in implementing SLAM. The hope is thus to present the subject in a clear and concise manner while keeping the prerequisites required to understand the document to a minimum. It should actually be 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 of different hardware that can be used. Secondly SLAM is more like a concept than a single algorithm. There are many steps involved in SLAM and these different steps can be implemented using a number of different algorithms. In most cases we explain a single approach to these different steps but hint at other possible ways to do them for the purpose of further reading.

The motivation behind writing this paper is primarily to help ourselves understand SLAM 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 primarily focus on innovations in small areas of SLAM, which of course is their purpose. The purpose of this paper is to be very practical and focus on a simple, basic SLAM algorithm that can be used as a starting point to get to know SLAM better. For people with some background knowledge in SLAM we here present a complete solution for SLAM 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 implementation up and running. It must also be noted that SLAM as such has not been completely solved 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 of downloading it, compiling it, plugging in the hardware (SICK laser scanner, ER1 robot) and executing the application; Plug-and-Play. We have used Microsoft Visual

4

C# and the code will compile in the .Net Framework v. 1.1. Most of the code is very straightforward and can be read almost as pseudo-code, so porting to other languages or platforms should be easy.

5

3.

About SLAM

The term SLAM is as stated an acronym for Simultaneous Localization And Mapping. 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 and Leonard 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 environment by a mobile robot while at the same time navigating the environment using the map.

SLAM consists of multiple parts; Landmark extraction, data association, state estimation, state update and landmark update. There are many ways to solve each of the smaller parts. We will be showing examples for each part. This also means that some of the parts can be replaced by a new way of doing this. As an example we will solve the landmark extraction problem in two different ways and comment on the methods. The idea is that you can use our implementation and extend it by using your own novel approach to these algorithms. We have decided to focus on a mobile robot in an indoor environment. You may choose to change some of these algorithms so that it can be for example used in a different environment.

SLAM is applicable for both 2D and 3D motion. We will only be considering 2D motion.

It is helpful if the reader is already familiar with the general concepts of SLAM on an introductory level, e.g. through a university level course on the subject. There are lots of great introductions to this field of research including [6][4]. Also it is helpful to know 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 easily understand this tutorial but it is not strictly required to comprehend all of it.

6

4.

The Hardware

The hardware of the robot is quite important. To do SLAM there is the need for a mobile robot and a range measurement device. The mobile robots we consider are wheeled indoor robots. This documents focus is mainly on software implementation of SLAM and does not explore robots with complicated motion models (models of how 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 on mobile robots.

The robot

Important parameters to consider are ease of use, odometry performance and price. The odometry performance measures how well the robot can estimate its own position, just from the rotation of the wheels. The robot should not have an error of more than 2 cm per meter moved and 2 per 45 degrees turned. Typical robot drivers allow the robot to report its (x,y) position in some Cartesian coordinate sys