Upload
omargw
View
252
Download
0
Embed Size (px)
Citation preview
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 2/66
Simultaneous Localization and Mapping
Ibraam Mahroos Fekri Mostafa Hassan Khamees
Omar Shaaban Ibrahim
Dr. Hassan Ali Hassan
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 3/66
Contents
• Introduction
• Where we are ?
• SLAM Problem
• Objectives
• Linear Dynamic Systems
• Extended Kalman Filter
• EKF SLAM Algorithm
• Data Association• Computer Vision
• Work
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 4/66
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.
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 5/66
Where we are?
14/04/2012SLAM5
Localization
Mapping
Path
Planning
SLAM
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 6/66
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.
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 7/66
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.
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 8/66
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.).
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 9/66
Sensor model
14/04/2012SLAM9
Measurement model
=
measurements vector at instance time
: unknown system noise (imprecision in theobservation process and/or model)
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 10/66
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)
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 11/66
Bayesian recursive estimation
14/04/2012SLAM11
|− = − , −) − − Prediction
= ) |− Update
No analytical solution in general
Linear Gaussian case (additive white Gaussian
noise, AWGN)
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 12/66
Kalman Filter
14/04/2012SLAM12
= − + − + − = + Prediction
− = − + −
− = −
Update
= −(− )−
= − ( −) = ( )−
MMSE Estimator
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 13/66
Extended Kalman Filter
14/04/2012SLAM13
Nonlinear dynamic systems
+= ( , ) = ( )
Linear approximation of dynamic model andmeasurement model
Prediction − = (− , − ) − = − −
Update = −(− )−
= − ( (−)) = ( )−
Best linear Estimator
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 14/66
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.
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 15/66
14/04/2012SLAM15
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 16/66
EKF-SLAM Algorithm cont.
14/04/2012SLAM16
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 17/66
Data Association
14/04/2012SLAM17
Data association consists in relating sensorobservations with the elements included in the map.
Interpretation tree.
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 18/66
Data Association cont.
14/04/2012SLAM18
Individual Compatibility Nearest Neighbor Algorithm (ICNN)
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 19/66
Why data association is so difficult?
14/04/2012SLAM19
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 20/66
Why data association is so difficult?
14/04/2012SLAM20
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 21/66
Why data association is so difficult?
14/04/2012SLAM21
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 22/66
Why Vision ?
»Human use vision
» Exclusive details
»Cheaper
14/04/2012SLAM22
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 23/66
Perception
» Ambient environment!
» Acquire knowledge
»Communicate» Interact
14/04/2012SLAM23
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 24/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 25/66
Vision Sensors Tech.
»CCD
Charged Coupled Device
»CMOSComplementary Metal Oxide Semi
Conductor
14/04/2012SLAM25
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 26/66
Mono Vs. Stereo
»Details
» 2D or 3D
»Range of Vision» Estimated Vs. Virtual 3D
14/04/2012SLAM26
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 27/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 28/66
Depth Estimation
»Depth needed for interactions
»Depth for stereo using Triangulation
14/04/2012SLAM28
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 29/66
Depth Estimation
14/04/2012SLAM29
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 30/66
Depth Estimation
14/04/2012SLAM30
= 2
= 2
=
;
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 31/66
Vision Algorithms
14/04/2012SLAM31
»Detection
»Description
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 32/66
Harris Algorithm
14/04/2012SLAM32
» Feature Detector
» Affine-Invariant
» Corner Measure» Gaussian Scale-Space
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 33/66
Corner Measure
14/04/2012SLAM33
= ,
,
, = , , =1
2 ( − +
)
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 34/66
Gaussian Scale-Space
14/04/2012SLAM34
, = ⨂ . 6.7
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 35/66
Harris Algorithm
14/04/2012SLAM35
»Combination between Corner Measure
and Scale-space
» Scale-Invariant detector
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 36/66
SIFT Algorithm
14/04/2012SLAM36
»S cale I nvariant F eature T ransform
»Detector and descriptor
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 37/66
Objects
14/04/2012SLAM37
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 38/66
Image
14/04/2012SLAM38
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 39/66
Result
14/04/2012SLAM39
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 40/66
Algorithm
14/04/2012SLAM40
»Constructing Scale-Space
» LOG Approximation
» Finding Keypoints» Accurate Keypoints
» Assigning Keypoints Orientation
»Generating SIFT Features
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 41/66
Scale-Space
14/04/2012SLAM41
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 42/66
Laplacian of Gaussian
14/04/2012SLAM42
, , = , , ∗ ,
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 43/66
Laplacian of Gaussian
14/04/2012SLAM43
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 44/66
Finding Keypoints
14/04/2012SLAM44
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 45/66
Finding Keypoints
14/04/2012SLAM45
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 46/66
Finding Keypoints
14/04/2012SLAM46
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 47/66
Accurate Keypoints
14/04/2012SLAM47
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 48/66
Descripting
14/04/2012SLAM48
» Assign orientations for Keypoints
»Generate SIFT features
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 49/66
Hardware
14/04/2012SLAM49
» Stereo camera
»Mono camera
» IP camera» 2 Mono = Stereo
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 50/66
Results
14/04/2012SLAM50
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 51/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 52/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 53/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 54/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 55/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 56/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 57/66
Arduino UNO
Advantages:
• Provide a simple interface.
• Easy to program
• USB over current protection
14/04/2012SLAM57
Direction and Speed Controller
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 58/66
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
Direction and Speed Controller
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 59/66
» Advantages:
The motor shield lets youcontrol a whole bunch ofcurrent (good for motors)with an itty-bitty signal(good for Arduinos).
14/04/2012SLAM59
Direction and Speed Controller
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 60/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 61/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 62/66
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
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 63/66
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 64/66
Future Work
» Autonomous
» Sensor Fusion
» Embedded Processing
» Better Estimator (for large scale environment)
14/04/2012SLAM64
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 65/66
Problems
8/4/2019 Simultaneous Localization and Mapping (SLAM)
http://slidepdf.com/reader/full/simultaneous-localization-and-mapping-slam 66/66
DEMO