T&C LAB-AI
Robotics
Probabilistic Robotics
Map Building in 2D
양정연
2020/12/10
1
T&C LAB-AI
Robotics
Map Update Strategy
with Inverse measurement(or Sensor) model
• If we calculate , then we update a map
• It is called
– From the current position, x and the current measurement, z,
we estimate prob. Of whether a map mi is empty or occupied.
It is different with a classification probability.
2
1:
1:
1: 1
1: 1
1
0
1
0
(
|
,
)
(
| , )
|
,
(
)
(
| , ) l
(
| , ) l
i
t
t
i
t
t
i
t
t
i
t
i
t
t
t
i
i
t
t
t
l m x
z
l m x z
l m x
z
l m
l
l m x z
l m
l m x z
l
1:
1:
0
0 : (
|
,
)
0 : (
)
i
t
t
t
i
t
p m x
z
l
t
p m
l
(
| , )
i
t
t
l m x z
(
| , )
(
| , )
Inverse measurement model
i
t
t
i
t
t
l m x z
p m x z
T&C LAB-AI
Robotics
Extend Map Update into 2Dim
• Map update with Inverse Sensor Model
• SLAM uses Distance Metric Sensor like laser scanner
• Try to understand Distance Metric Sensor
3
1
0
1
0
(
| , )
=InverseSensorModel(m, x, z)
t
i
t
t
t
t
l
l m x z
l
l
l
l
T&C LAB-AI
Robotics
Two Programs are Used
• loop.sys is a Python-based program is communicated
with Pathfinder.exe
– Internal process communication (ICP)
4
Path finder 3D environment
Pathfinder.exe
loop.sys
Sensor
Distance
information
Move(x,y,q)
T&C LAB-AI
Robotics
Example: Path Finder with Laser Scanner
• Add walls and Distance Sensor
5
T&C LAB-AI
Robotics
1. Wall and Sensor Model from XML
• Define wall, grid, and sensor model from “config.xml”
• Redefine Grid world, with 20x20
6
W=20
Grid =21
T&C LAB-AI
Robotics
2. Add Walls
• Use a virtual map (“map.bmp”)
7
Map.bmp
2D map with Photoshop
Path finder
3D wall
3D walls from 2D image
T&C LAB-AI
Robotics
3. Virtual Sensor
• Define Sensor type from “config.xml”
8
0
90
start
90
end
45
start
45
end
3
resolution
T&C LAB-AI
Robotics
3. Virtual Sensor Data
• High Quality Sensor has 0.5 degree resolution
– Distance error is about 1mm.
9
T&C LAB-AI
Robotics
Ex1) Distance Metric with Robot Movements
• testpfm.pf.move(0,0,0) testpfm.pf.move(5,0,0)
10
• testpf.pfm.move(0,0,0) testpf.pfm.move(0,0,30)
T&C LAB-AI
Robotics
2D Map Update
• Log Odds-based Map Update
• Suppose that One Map, with one Distance Sensor,
11
1:
1:
1: 1
1: 1
1
0
1
0
(
|
,
)
(
| , )
|
,
(
)
(
| , ) l
l
i
t
t
i
t
t
i
t
t
i
t
i
t
t
t
t
l m x
z
l m x z
l m x
z
l m
l
l m x z
l
InverseSensorModel
l
i
m
mi
ˆx
i
s
1
i
s
1
i
s
i
s
T&C LAB-AI
Robotics
Check Condition and
• 1. Find the angle,
12
i
m
i
s
mi
ˆx
ˆ [ , , ]T
x
x y
: heading angle
atan2(
- ,
- )
ix
iy
m
x m
y
2
2
,
,
ˆ
|| m -x||= (
)
(
)
i
i x
i y
r
m
x
m
y
T&C LAB-AI
Robotics
Check Condition and
• 2. Find the best closest Sensor
13
i
m
i
s
i
s
1
i
s
1
i
s
mi
ˆx
ˆ [ , , ]T
x
x y
arg min(|
s
|)
s
i
i
i
| |
k
z
norm
T&C LAB-AI
Robotics
14
i
s
k
z
mi
i
s
k
z
mi
Map, mi is out of sensor area
Map, mi is similar to sensor
data
Case 1) We Don’t
Know it
Use Sensor Model
3. Check Distance
Case 2) Occupancy Update
i
s
k
z
mi
Case 3) Emptiness Update
Map, mi is inside of sensor
area
T&C LAB-AI
Robotics
Sensor Probability Model
15
distance
Prob. Of occupancy, p(x)
0.2
10m
0.8
a
i
s
k
z
mi
r
Empty
Occ
We don’t
know(Uncertain)
Uncertain case
Occupant case
Empty case
T&C LAB-AI
Robotics
16
Case 1)
Uncertain
Use Sensor Model
3. Check Distance
Case 2)
Occupancy Update
Case 3)
Emptiness Update
0
(m)
log Odds : (m)
log
1
(m)
p
l
p
0.5
0
log
0
1 0.5
l
0.8
log
1 0.8
occ
l
0.2
log
1 0.2
empty
l
T&C LAB-AI
Robotics
Use Sensor Model
4. Check Sensor Width
17
i
s
1
i
s
mi
1
i
s
Sensor
Resolution, b
=const.
b
|
s
|
2
i
b
From Condition 1.
i
s
Uncertain
Uncertain
mi
mj
mk
If mi is out of sensor width, then it is uncertain and use l0.
T&C LAB-AI
Robotics
Ex2) Map Update with Linear Motion
• Map Update for every mi (0<i<20 and 0<j<20)
18
1
0
1
0
(
| , ) l
l
t
i
t
t
t
t
l
l m x z
l
InverseSensorModel
l
T&C LAB-AI
Robotics
Ex2) testpfm.py
19
Get sensor data from pathfinder
Sensor Noise with 0.5 Gaussian
Start angle=-90
End angle=90
Resolution=3 deg.
Thus, (90-(-90))/3
60 + 1 (end angle)
ret=61 Distance Array
Whenever pf moves,
Call updatemap()
T&C LAB-AI
Robotics
Ex2) testpfm.py
20
T&C LAB-AI
Robotics
Ex2) testpfm.py move(-5,0,0)
21
T&C LAB-AI
Robotics
HW) Complete Your Map Building
• Testpfm.py DOES NOT consider heading angle
– Rotation is NOT considered.
• Complete your mapping with regarding to Rotation.
– Think heading angle..
22
Hw.
Mapping is
Hard work!
T&C LAB-AI
Robotics
HW 2) Complete Your Map Building
• Testpfm.py DOES NOT consider heading angle
– Rotation is NOT considered.
• Complete your mapping with regarding to Rotation.
– Think heading angle..
23
