simultaneous localization and mapping (slam)

8/4/2019 Simultaneous Localization and Mapping (SLAM)

http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 1/66

Minya UniversityFaculty of Engineering

CSE 2011



Simultaneous Localization and Mapping

Ibraam Mahroos Fekri Mostafa Hassan Khamees

Omar Shaaban Ibrahim

Dr. Hassan Ali Hassan



Contents

• Introduction

• Where we are ?

• SLAM Problem

• Objectives

• Linear Dynamic Systems

• Extended Kalman Filter

• EKF SLAM Algorithm

• Data Association• Computer Vision

• Work



Introduction

14/04/2012SLAM4

Localization Determine position given a priori map of the environment.

Mapping

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



Where we are?

14/04/2012SLAM5

Localization

Mapping

Path

Planning

SLAM



SLAM Problem

14/04/2012SLAM6

Simultaneous Localization And Mapping (SLAM)

An autonomous vehicle exploring an unknown environmentwith onboard sensor and incrementally build a map of thisenvironment while simultaneously using this map to

computing the vehicle location.

SLAM combines the two problems of localization andmapping.



Objective

14/04/2012SLAM7

Solving the SLAM problem in a static environmentusing vision sensor and Extended Kalman Filter as

an estimator.

Given Robot’s controls.

Onboard sensor (stereo camera).

Estimate

Map representation of the environment (feature basedmap).

Trajectory of the robot.



Linear dynamic systems

14/04/2012SLAM8

Dynamic model:

+=

: State vector of the system at instant t = k T (assumed to be complete, Markov assumption).

: known control inputs.

: unknown system noise (can be due to systemperturbations, model imperfections…etc.).



Sensor model

14/04/2012SLAM9

Measurement model

=

measurements vector at instance time

: unknown system noise (imprecision in theobservation process and/or model)



Optimal Estimation

14/04/2012SLAM10

Available information up to time k:

= * , , 3 , … U− = * u, u , 3 , … −

Assuming independent white Gaussian noises for thestate and the measurement, this system can besummarized in these conditional PDFs:

|− ≜ −, − Prediction ≜(| , −) Estimation

Optimal estimation(minimum mean squared error)



Bayesian recursive estimation

14/04/2012SLAM11

|− = − , −) − − Prediction

= ) |− Update

No analytical solution in general

Linear Gaussian case (additive white Gaussian

noise, AWGN)



Kalman Filter

14/04/2012SLAM12

= − + − + − = + Prediction

− = − + −

− = −

Update

= −(− )−

= − ( −) = ( )−

MMSE Estimator



Extended Kalman Filter

14/04/2012SLAM13

Nonlinear dynamic systems

+= ( , ) = ( )

Linear approximation of dynamic model andmeasurement model

Prediction − = (− , − ) − = − −

Update = −(− )−

= − ( (−)) = ( )−

Best linear Estimator



EKF-SLAM Algorithm

14/04/2012SLAM14

Consider a mobile robot moving through anenvironment taking relative observations of a

number of unknown landmarks using a sensorlocated on the robot.



14/04/2012SLAM15



EKF-SLAM Algorithm cont.

14/04/2012SLAM16



Data Association

14/04/2012SLAM17

Data association consists in relating sensorobservations with the elements included in the map.

Interpretation tree.



Data Association cont.

14/04/2012SLAM18

Individual Compatibility Nearest Neighbor Algorithm (ICNN)



Why data association is so difficult?

14/04/2012SLAM19




14/04/2012SLAM20




14/04/2012SLAM21



Why Vision ?

»Human use vision

» Exclusive details

»Cheaper

14/04/2012SLAM22



Perception

» Ambient environment!

» Acquire knowledge

»Communicate» Interact

14/04/2012SLAM23



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

14/04/2012SLAM24



Vision Sensors Tech.

»CCD

Charged Coupled Device

»CMOSComplementary Metal Oxide Semi

Conductor

14/04/2012SLAM25



Mono Vs. Stereo

»Details

» 2D or 3D

»Range of Vision» Estimated Vs. Virtual 3D

14/04/2012SLAM26



Camera Calibration

»Why be done?

» Intrinsic parameters

» Focal length» Principal point

» Extrinsic parameters

» denote the coordinate system transformation from3D world coordinate to camera 3D coordinates

14/04/2012SLAM27



Depth Estimation

»Depth needed for interactions

»Depth for stereo using Triangulation

14/04/2012SLAM28



Depth Estimation

14/04/2012SLAM29



Depth Estimation

14/04/2012SLAM30

= 2

= 2

=

;



Vision Algorithms

14/04/2012SLAM31

»Detection

»Description



Harris Algorithm

14/04/2012SLAM32

» Feature Detector

» Affine-Invariant

» Corner Measure» Gaussian Scale-Space



Corner Measure

14/04/2012SLAM33

= ,

,

, = , , =1

2 ( − +

)



Gaussian Scale-Space

14/04/2012SLAM34

, = ⨂ . 6.7



Harris Algorithm

14/04/2012SLAM35

»Combination between Corner Measure

and Scale-space

» Scale-Invariant detector



SIFT Algorithm

14/04/2012SLAM36

»S cale I nvariant F eature T ransform

»Detector and descriptor



Objects

14/04/2012SLAM37



Image

14/04/2012SLAM38



Result

14/04/2012SLAM39



Algorithm

14/04/2012SLAM40

»Constructing Scale-Space

» LOG Approximation

» Finding Keypoints» Accurate Keypoints

» Assigning Keypoints Orientation

»Generating SIFT Features



Scale-Space

14/04/2012SLAM41



Laplacian of Gaussian

14/04/2012SLAM42

, , = , , ∗ ,



Laplacian of Gaussian

14/04/2012SLAM43



Finding Keypoints

14/04/2012SLAM44



Finding Keypoints

14/04/2012SLAM45



Finding Keypoints

14/04/2012SLAM46



Accurate Keypoints

14/04/2012SLAM47



Descripting

14/04/2012SLAM48

» Assign orientations for Keypoints

»Generate SIFT features



Hardware

14/04/2012SLAM49

» Stereo camera

»Mono camera

» IP camera» 2 Mono = Stereo



Results

14/04/2012SLAM50



Mobile robot

Is two wheel differential driverobot(2WD) controlled byarduino and motor shieldfrom PC, laptop and mobileover Wi-Fi network usingWiFly shield.

The robot has two camera towork as stereo camera andtwo quadrature wheel

encoders to measure thedistance each wheel hastraveled.

14/04/2012SLAM51



Mobile robot

Advantages of differential drive mobile robot:

One of the simplest mobile robot constructions is achassis with two fixed wheels.

It can rotate freely in all directions Simple kinematic model

14/04/2012SLAM52



The Robot Kinematic Model

V L is the left wheel’s linear

velocity.

V R is the right wheel’slinear velocity.

V S is the robot linear velocity

D is the wheel base Ө is the robot angle

14/04/2012SLAM53



Robot Equation of Motion

=++

+

=

( )cos()

( )sin() ( )

Where X, Y are the position of the robot with respect

to global frame and Ө is the angle between axis Xand the robot axis.

14/04/2012SLAM54



Robot Controller

Using Arduino UNO

It's an open-source physical computing platform based

on a simple microcontroller board, and a developmentenvironment for writing software for the board.

14/04/2012SLAM55



Arduino UNO

Is a microcontroller boardbased on the ATmega328and it has:

14 digital I/O pins (of

which 6 can be used asPWM outputs)

6 analog inputs

16 MHz crystal oscillator

USB connection

Power jack, ICSP header,and a reset button.

14/04/2012SLAM56



Arduino UNO

Advantages:

• Provide a simple interface.

• Easy to program

• USB over current protection

14/04/2012SLAM57

Direction and Speed Controller




» Is Arduino Motor shielduses L298P h-bridgechip which allow to drivetwo 7-12V DC motors

with maximum 2Acurrent.

» This shield can bedirectly mount ontostandard Arduino UNO,Duemilanove andArduino Mega.

14/04/2012SLAM58




» Advantages:

The motor shield lets youcontrol a whole bunch ofcurrent (good for motors)with an itty-bitty signal(good for Arduinos).

14/04/2012SLAM59




Communication

» Using WiFly Shield» Arduino able to connect

to 802.11b/g wirelessnetworks.

» The featuredcomponents of the shieldare a Roving Network'sRN-131G wireless

module and anSC16IS750 SPI-to-UARTchip.

14/04/2012SLAM60



Controlling the Robot

Programming the Controller:

» Write program making Arduino work as a web serverstoring an HTML page.

» Upload the program to Arduino .

14/04/2012SLAM61



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

14/04/2012SLAM62



Future Work

» Autonomous

» Sensor Fusion

» Embedded Processing

» Better Estimator (for large scale environment)

14/04/2012SLAM64



Problems



DEMO

simultaneous localization and mapping (slam)

Documents