Post on 16-Sep-2018
Offline Simultaneous Localization and Offline Simultaneous Localization and Mapping (SLAM) using Miniature Robots Mapping (SLAM) using Miniature Robots
• Objectives• SLAM approaches• SLAM for ALICE
– EKF for Navigation– Mapping and Network Modeling
• Test results
Philipp Schaer and Adrian WaegliJune 29, 2007
ALICE ALICE --ROBOTROBOT
• Alice As cheap and small as possible– 2 SWATCH motors with wheels and tires (max speed: 40mm/s) – 4 active IR proximity sensors (reflection measurement)– Microcontroller with 8Kwords Flash program memory – Radio communication module
22mm
20m
m
60°4cm
Obj
ectiv
es
1/2
Test Environment Test Environment -- Centimeter range labyrinthCentimeter range labyrinth
Obj
ectiv
es
2/2
General SLAM ApproachesGeneral SLAM Approaches
Localization• Kalman filter (EKF, UKF)• Particle filter• IA methods (Fuzzy logic, neural
networks…)
Gen
eral
SLA
M A
ppro
ache
sMapping• Grid maps• Feature recognition / scan
matches• Tree-based network optimizers
SLAM for ALICESLAM for ALICE
• EKF for navigation• 3 approaches for mapping:
– Grid mapping– Boundary detection based on range measurements– Network reconstruction
SLA
M fo
r ALI
CE
1/7
System Process Model System Process Model
1
1
1
sin2
cos2
rk k k
rk k k
rk k
d dx x
d dy y
d dD
θ
θ
θ θ
+
+
+
+= +
+= +
−= +
l
l
l
1 0 cos2
0 1 sin2
0 0 1
r
r
d d
d d
θ
θ
+⎡ ⎤⎢ ⎥⎢ ⎥
+⎢ ⎥= −⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦
F
l
l
2
2
2
x
y
θ
σσ
σ
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
P
2
2d
dr
σσ
⎡ ⎤= ⎢ ⎥⎣ ⎦
wwQ l
sin sin2 2
cos cos2 21 1D D
θ θ
θ θ
⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥= ⎢ ⎥⎢ ⎥⎢ ⎥−⎢ ⎥⎣ ⎦
Γ
Functional model:
Stochastic model:
x
y
θdl
rdD
SLA
M fo
r ALI
CE
3/7
Measurement Model Measurement Model
0
320SLAM
k
θ
θ −
= °
= °
10.4
10.5
SLAM
k
xy
xy
−
⎡ ⎤ ⎡ ⎤=⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
⎡ ⎤ ⎡ ⎤=⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
1 0 00 1 0⎡ ⎤
= ⎢ ⎥⎣ ⎦
posH
[ ]0 0 1=dirH
( )ˆk
SLAM
xh x y
θ
−
⎡ ⎤⎢ ⎥= ⎢ ⎥⎢ ⎥⎣ ⎦
2
2x
y
σσ
⎡ ⎤= ⎢ ⎥⎢ ⎥⎣ ⎦
posR
2θσ⎡ ⎤= ⎣ ⎦dirR
90
110SLAM
k
θ
θ −
= °
= °
x
y0.5
0
0 1 2
SLA
M fo
r ALI
CE
4/7
∆l
Environment ModelingEnvironment Modeling
• Robots programmed with “follow & avoidance” behavior• Online calibration of the wheel data not feasible
SLA
M fo
r ALI
CE
6/7x
y
• Estimate network nodes and segments• Appropriate for Labyrinth mapping• Poor IR sensor performance (unusable for Navigation)• Map matching for navigation
Network ModelingNetwork Modeling
SLA
M fo
r ALI
CE
7/7
Test SetupTest Setup
Test
Res
ults
• Construction of “Labyrinth” environment • 3 Runs of ca 2min with 3 different robots• Robots programmed with “follow &
avoidance” behavior• Odometry and range data transmission
every 15 sec by wireless communication• Decoding of HEX-data• SLAM implemented in MATLAB
1/6
RawRaw odometryodometry Data / Calibration issuesData / Calibration issues
Test
Res
ults
• Odometry data: – Registered ticks on left and right wheel conversion of ticks into distance– Calibration: tick conversion values are not exactly known– Transmission errors in data packages need to “correct” and “cut” data
2/6Left tick = right tick Left tick = 0.98* right tick
Max
. pos
sibl
e sp
eed
(40m
m/s
Length of one data block
Uncorrected data Corrected data
x
y
RawRaw sensorsensor DataData
• Raw IR sensor data: – Conversion of ADC value into range value– Computation of target coordinates (georeferencing)– Feature detection edges, walls
Test
Res
ults
3/6
xc(i),yc(i)
αsensor
γ(i)
xt(i),yt(i)
asensor
cos( ) sin( ) sin( )*
sin( ) cos( ) cos( )i i sensor
ii i se
sensoi
orr
i n
i
i s
xa
Xr
Yyλ λ αλ λ α
⎛ ⎞⎡ ⎤ ⎡ ⎤= + +⎜ ⎟⎢ ⎥ ⎢ ⎥⎜ ⎟−⎣ ⎦ ⎣
⎡ ⎤
⎦⎝⎢
⎡ ⎤⎢⎣
⎥⎦ ⎦⎣ ⎠
⎥r
for sensor = left,front,right,back
r(i)
leftfront rightback
GridGrid ModellingModelling ((OccupancyOccupancy GridGrid MapMap))
Test
Res
ults
4/6
• Map robot position and georeferenced target directly on grid• Labyrinth reconstruction requires feature recognition• Feature recognition difficult because of the quality of the
– Odometer data– Range measurements
• Scan matching impossible– “Random” measurements in corners– Data gaps
BoundaryBoundary DetectionDetection
• Edges are not directly observable• Fit lines by least squares while
robot in “follow wall” behavior• Evaluate turn rate and identify type
of turn:– Left turn (90°)– Right turn (90°)– U-turn (180°)
• Attitude update after each turn Test
Res
ults
5/6Problem: position uncertainty still growingTrack
Estimated std
Left scan
Right scan
Network Network ModellingModelling ApproachApproach
• Localization: It is sufficient to know in which corridor robot is
• Mapping: Construct topological network of the environment
• Identify known network points position update possible
• Attitude update after each turn
Test
Res
ults
6/6Position uncertainty reduced
Track
Estimated std
new / matched network point
New / matched network segment
ConclusionConclusion
• Grid estimation approach not applicable:– Feature recognition and scan matching almost impossible
• Boundary detection possible, but cumbersome:– Implicit boundary detection by “follow wall” behavior (robots runs on straight
line)– Poor quality of georeferencing in turns – Difficult to implement position updates
• Network estimation seems to be the most appropriate method for labyrinth mapping for the given sensors:– More robust in turns– Easier topology recognition– Loop closure at every matched network point– Map matching along line segments
Con
clus
ion
1/2
OutlookOutlook
• After several passes through the same network, fewer assumptions might be required:– Store and verify topological beliefs– Direction and position updates later, but no assumption with respect
to the network angles necessary
Con
clus
ion
2/2( ) 45%p LR =
Left Straight Right BackTurn 1
Turn 2 L S R B L S R B L S R B L S R B
( ) 15%p LL =( ) 35%p LS = ( ) 5%p LB =Trajectory Network guesses