Minya University

Faculty of Engineering CSE 2011

Simultaneous Localization and Mapping

Dr. Hassan Ali Hassan

Ibraam Mahroos Fekri Mostafa Hassan Khamees Omar Shaaban Ibrahim

Contents
Introduction Where we are ? SLAM Problem Objectives Linear Dynamic Systems Extended Kalman Filter EKF SLAM Algorithm Data Association Computer Vision Work

Introduction

Localization

Determine position given a priori map of the environment.

Mapping

Generating map of the environment when the pose is accurately known from auxiliary source.

SLAM

01/04/2014

Where we are?
SLAM
Localization

Path Planning

Mapping

SLAM

01/04/2014

SLAM Problem

Simultaneous Localization And Mapping (SLAM)

An autonomous vehicle exploring an unknown environment with onboard sensor and incrementally build a map of this environment while simultaneously using this map to computing the vehicle location.
SLAM combines the two problems of localization and mapping.

SLAM

01/04/2014

Objective

Solving the SLAM problem in a static environment using vision sensor and Extended Kalman Filter as an estimator. Given

Robots controls. Onboard sensor (stereo camera).

Map representation of the environment (feature based map). Trajectory of the robot.
SLAM 01/04/2014

Estimate

Linear dynamic systems

Dynamic model:

+1 = + +
: State vector of the system at instant t = kT (assumed to be complete, Markov assumption). : known control inputs.

k : unknown system noise (can be due to system perturbations, model imperfectionsetc.).

8 SLAM 01/04/2014

Sensor model

Measurement model

= +
measurements vector at instance time : unknown system noise (imprecision in the observation process and/or model)

SLAM

01/04/2014

Optimal Estimation

Available information up to time k:

= * 1 , 2 , 3 , + U k1 = * u1 , u2 , 3 , 1 +

Assuming independent white Gaussian noises for the state and the measurement, this system can be summarized in these conditional PDFs:
|1 1 , 1 Prediction

( | , 1 )

Estimation

Optimal estimation(minimum mean squared error)

10 SLAM 01/04/2014

Bayesian recursive estimation

|1 = 1 , 1 ) 1 1 Prediction Update = ) |1

No analytical solution in general Linear Gaussian case (additive white Gaussian noise, AWGN)

11

SLAM

01/04/2014

Kalman Filter

= 1 + 1 + 1 = + Prediction = 1 + 1 = 1 + Update = ( + )1 = + ( ) = ( ) MMSE Estimator

12 SLAM 01/04/2014

Extended Kalman Filter

Nonlinear dynamic systems

+1 = ( , ) + = ( ) +

Linear approximation of dynamic model and measurement model Prediction

= (1 , 1 ) = 1 + 1

Update
1 = ( + ) = + ( ( )) = ( )

Best linear Estimator

13 SLAM 01/04/2014

EKF-SLAM Algorithm

Consider a mobile robot moving through an environment taking relative observations of a number of unknown landmarks using a sensor located on the robot.

14

SLAM

01/04/2014

15

SLAM

01/04/2014

EKF-SLAM Algorithm cont.

16

SLAM

01/04/2014

Data Association

Data association consists in relating sensor observations with the elements included in the map. Interpretation tree.

17

SLAM

01/04/2014

Data Association cont.

Individual Compatibility Nearest Neighbor Algorithm (ICNN)

18

SLAM

01/04/2014

Why data association is so difficult?

19

SLAM

01/04/2014

Why data association is so difficult?

20

SLAM

01/04/2014

Why data association is so difficult?

21

SLAM

01/04/2014

Why Vision ?
Human

use vision Exclusive details Cheaper

22

SLAM

01/04/2014

Perception
Ambient

environment! Acquire knowledge Communicate Interact

23

SLAM

01/04/2014

Sensors

Why To Sense ? Proprioceptive / Exteroceptive Proprioceptive: Motor speed Wheel Load Exteroceptive: Light intensity - Distance Active / Passive

Active: Wheel quadrature encoder Passive: Microphones CMOS camera

24

SLAM

01/04/2014

Vision Sensors Tech.

CCD CMOS

Charged Coupled Device

Complementary Metal Oxide Semi Conductor

25

SLAM

01/04/2014

Mono Vs. Stereo

Details 2D

or 3D Range of Vision Estimated Vs. Virtual 3D

26

SLAM

01/04/2014

Camera Calibration
Why

be done? Intrinsic parameters

Focal length Principal point

Extrinsic

parameters

denote the coordinate system transformation from 3D world coordinate to camera 3D coordinates

27

SLAM

01/04/2014

Depth Estimation
Depth

needed for interactions Depth for stereo using Triangulation

28

SLAM

01/04/2014

Depth Estimation

29

SLAM

01/04/2014

Depth Estimation
+ 2 =

+ 2 =
= ;
30 SLAM 01/04/2014

Vision Algorithms
Detection Description

31

SLAM

01/04/2014

Harris Algorithm
Feature

Detector Affine-Invariant

Corner Measure Gaussian Scale-Space

32

SLAM

01/04/2014

Corner Measure
=
,

2 ,

2
2 + 2 ( 2 ) 2

1 , = , , = 2 2

33

SLAM

01/04/2014

Gaussian Scale-Space

, =

. 6.7

34

SLAM

01/04/2014

Harris Algorithm
Combination

between Corner Measure and Scale-space Scale-Invariant detector

35

SLAM

01/04/2014

SIFT Algorithm
Scale Detector

Invariant Feature Transform

and descriptor

36

SLAM

01/04/2014

Objects

37

SLAM

01/04/2014

Image

38

SLAM

01/04/2014

Result

39

SLAM

01/04/2014

Algorithm
Constructing

Scale-Space LOG Approximation Finding Keypoints Accurate Keypoints Assigning Keypoints Orientation Generating SIFT Features
40 SLAM 01/04/2014

Scale-Space

41

SLAM

01/04/2014

Laplacian of Gaussian

, , = , , ,

42

SLAM

01/04/2014

Laplacian of Gaussian

43

SLAM

01/04/2014

Finding Keypoints

44

SLAM

01/04/2014

Finding Keypoints

45

SLAM

01/04/2014

Finding Keypoints

46

SLAM

01/04/2014

Accurate Keypoints

47

SLAM

01/04/2014

Descripting
Assign

orientations for Keypoints Generate SIFT features

48

SLAM

01/04/2014

Hardware
Stereo

camera Mono camera IP camera 2 Mono = Stereo

49

SLAM

01/04/2014

Results

50

SLAM

01/04/2014

Mobile robot
Is two wheel differential drive robot(2WD) controlled by arduino and motor shield from PC, laptop and mobile over Wi-Fi network using WiFly shield. The robot has two camera to work as stereo camera and two quadrature wheel encoders to measure the distance each wheel has traveled.

51

SLAM

01/04/2014

Mobile robot
Advantages of differential drive mobile robot:

One of the simplest mobile robot constructions is a chassis with two fixed wheels. It can rotate freely in all directions Simple kinematic model

52

SLAM

01/04/2014

The Robot Kinematic Model

VL is the left wheels linear velocity. VR is the right wheels linear velocity. VS is the robot linear velocity D is the wheel base is the robot angle

53

SLAM

01/04/2014

Robot Equation of Motion

+1 = +1 = +1
1 + ( + )cos( ) 2 1 + ( + )sin( ) 2 1 + ( )

Where X, Y are the position of the robot with respect to global frame and is the angle between axis X and the robot axis.

54

SLAM

01/04/2014

Robot Controller
Using Arduino

UNO

It's an open-source physical computing platform based on a simple microcontroller board, and a development environment for writing software for the board.

55

SLAM

01/04/2014

Arduino UNO
Is a microcontroller board based on the ATmega328 and it has: 14 digital I/O pins (of which 6 can be used as PWM outputs) 6 analog inputs 16 MHz crystal oscillator USB connection Power jack, ICSP header, and a reset button.
56 SLAM 01/04/2014

Arduino UNO
Advantages: Provide a simple interface. Easy to program USB over current protection

57

SLAM

01/04/2014

Direction and Speed Controller

Is Arduino Motor shield uses L298P h-bridge chip which allow to drive two 7-12V DC motors with maximum 2A current. This shield can be directly mount onto standard Arduino UNO, Duemilanove and Arduino Mega.
58 SLAM 01/04/2014

Direction and Speed Controller

Advantages: The motor shield lets you control a whole bunch of current (good for motors) with an itty-bitty signal (good for Arduinos).

59

SLAM

01/04/2014

Communication

Using WiFly Shield Arduino able to connect to 802.11b/g wireless networks. The featured components of the shield are a Roving Network's RN-131G wireless module and an SC16IS750 SPI-to-UART chip.
60 SLAM 01/04/2014

Controlling the Robot

Programming the Controller: Write program making Arduino work as a web server storing an HTML page. Upload the program to Arduino .

61

SLAM

01/04/2014

Controlling the Robot

Plug in Arduino ,Wifly and Motor Shield power. Waitting WiFly shield to join the Wi-Fi network Open Internet browser Connect to Arduino server Browser send a request to Arduino server HTML page for contol Buttons loaded

62

SLAM

01/04/2014

Controlling the Robot

Press a Button (e.g. Froward) Browser Send Request to Arduino Arduino Receives The Request Arduino Send Control signals to Motor Shield Arduino Measures Encoder Data Server Waiting for New Request

63

SLAM

01/04/2014

Future Work

Autonomous Sensor Fusion Embedded Processing Better Estimator (for large scale environment)

64

SLAM

01/04/2014

Problems

DEMO