Probabilistic Robotics
description
Transcript of Probabilistic Robotics
![Page 1: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/1.jpg)
Probabilistic Robotics
Robot Localization
![Page 2: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/2.jpg)
2
Localization
• Given • Map of the environment.• Sequence of sensor measurements.
• Wanted• Estimate of the robot’s position.
• Problem classes• Position tracking• Global localization• Kidnapped robot problem (recovery)
“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
![Page 3: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/3.jpg)
3
Localization
![Page 4: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/4.jpg)
4
Localization
Position tracking
![Page 5: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/5.jpg)
5
Localization
Global localization
![Page 6: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/6.jpg)
6
Landmark-based Localization
![Page 7: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/7.jpg)
7
Linearity Assumption Revisited
![Page 8: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/8.jpg)
8
Non-linear Function
![Page 9: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/9.jpg)
9
EKF Linearization (1)
![Page 10: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/10.jpg)
10
EKF Linearization (2)
![Page 11: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/11.jpg)
11
EKF Linearization (3)
![Page 12: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/12.jpg)
12
•Prediction:
•Correction:
EKF Linearization: First Order Taylor Series Expansion
)(),(),(
)(),(
),(),(
1111
111
111
ttttttt
ttt
tttttt
xGugxug
xx
ugugxug
)()()(
)()(
)()(
ttttt
ttt
ttt
xHhxh
xx
hhxh
![Page 13: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/13.jpg)
13
EKF Algorithm
1. Extended_Kalman_filter( t-1, t-1, ut, zt):
2. Prediction:3. 4.
5. Correction:6. 7. 8.
9. Return t, t
),( 1 ttt ug
tTtttt RGG 1
1)( tTttt
Tttt QHHHK
))(( ttttt hzK
tttt HKI )(
1
1),(
t
ttt x
ugG
t
tt x
hH
)(
ttttt uBA 1
tTtttt RAA 1
1)( tTttt
Tttt QCCCK
)( tttttt CzK
tttt CKI )(
![Page 14: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/14.jpg)
14
1. EKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2.
3.
4.
5.
6.
),( 1 ttt ug Tttt
Ttttt VMVGG 1
,1,1,1
,1,1,1
,1,1,1
1
1
'''
'''
'''
),(
tytxt
tytxt
tytxt
t
ttt
yyy
xxx
x
ugG
tt
tt
tt
t
ttt
v
y
v
y
x
v
x
u
ugV
''
''
''
),( 1
2
42
3
22
21
0
0
tt
ttt
v
vM
Motion noise
Jacobian of g w.r.t location
Predicted mean
Predicted covariance
Jacobian of g w.r.t control
![Page 15: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/15.jpg)
15
1. EKF_localization ( t-1, t-1, ut, zt, m):
Correction:
2.
3.
4.
5.
6.
7.
8.
)ˆ( ttttt zzK
tttt HKI
,
,
,
,
,
,),(
t
t
t
t
yt
t
yt
t
xt
t
xt
t
t
tt
rrr
x
mhH
,,,
2,
2,
,2atanˆ
txtxyty
ytyxtxt
mm
mmz
tTtttt QHHS
1 tTttt SHK
2
2
0
0
r
rtQ
Predicted measurement mean
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
![Page 16: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/16.jpg)
16
EKF Prediction Step (known correspondences)
![Page 17: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/17.jpg)
17
EKF Correction Step (known correspondences)
![Page 18: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/18.jpg)
18
EKF Prediction Step (unknown correspondences)
![Page 19: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/19.jpg)
19
EKF Correction Step (unknown correspondences)
![Page 20: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/20.jpg)
20
EKF Prediction Step
![Page 21: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/21.jpg)
21
EKF Observation Prediction Step
![Page 22: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/22.jpg)
22
EKF Correction Step
![Page 23: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/23.jpg)
23
Estimation Sequence (1)
![Page 24: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/24.jpg)
24
Estimation Sequence (2)
![Page 25: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/25.jpg)
25
Comparison to GroundTruth
![Page 26: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/26.jpg)
26
EKF Summary
•Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376 + n2)
•Not optimal!•Can diverge if nonlinearities are large!•Works surprisingly well even when all
assumptions are violated!
![Page 27: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/27.jpg)
27
Linearization via Unscented Transform
EKF UKF
![Page 28: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/28.jpg)
28
UKF Sigma-Point Estimate (2)
EKF UKF
![Page 29: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/29.jpg)
29
UKF Sigma-Point Estimate (3)
EKF UKF
![Page 30: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/30.jpg)
30
Unscented Transform
média a relação em espalhados estão points sigma os distantes
quão determinam que parâmetros a iguais sendok e com ,)(
2,...,1for )(2
1 )(
)1(
2
2000
nkn
nin
wwn
nw
nw
ic
imi
i
cm
Sigma points Weights
)( ii g
n
i
Tiiic
n
i
iim
w
w
2
0
2
0
))(('
'
Pass sigma points through nonlinear function
Recover mean and covariance
![Page 31: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/31.jpg)
31
UKF_localization ( t-1, t-1, ut, zt, m):
Prediction:
2
42
3
22
21
0
0
tt
ttt
v
vM
2
2
0
0
r
rtQ
TTTt
at 000011
t
t
tat
Q
M
00
00
001
1
at
at
at
at
at
at 111111
xt
utt
xt ug 1,
L
i
T
txtit
xti
ict w
2
0,,
L
i
xti
imt w
2
0,
Motion noise
Measurement noise
Augmented state mean
Augmented covariance
Sigma points
Prediction of sigma points
Predicted mean
Predicted covariance
![Page 32: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/32.jpg)
32
UKF_localization ( t-1, t-1, ut, zt, m):
Correction:
zt
xtt h
L
iti
imt wz
2
0,ˆ
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance
Ttti
L
itti
ict zzwS ˆˆ ,
2
0,
Ttti
L
it
xti
ic
zxt zw ˆ,
2
0,
,
1, tzx
tt SK
)ˆ( ttttt zzK
Tttttt KSK
![Page 33: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/33.jpg)
33
UKF Prediction Step
![Page 34: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/34.jpg)
34
UKF Observation Prediction Step
![Page 35: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/35.jpg)
35
UKF Correction Step
![Page 36: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/36.jpg)
36
EKF Correction Step
![Page 37: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/37.jpg)
37
Estimation Sequence
EKF PF UKF
![Page 38: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/38.jpg)
38
Estimation Sequence
EKF UKF
![Page 39: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/39.jpg)
39
Prediction Quality
EKF UKF
![Page 40: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/40.jpg)
40
UKF Summary
•Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications
•Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term)
•Derivative-free: No Jacobians needed
•Still not optimal!
![Page 41: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/41.jpg)
41
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
Kalman Filter-based System
[Courtesy of Kai Arras]
![Page 42: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/42.jpg)
42
Map-based Localization
![Page 43: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/43.jpg)
43
Monte Carlo (Particle Filter) Localization
![Page 44: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/44.jpg)
44
Resampling Algorithm
![Page 45: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/45.jpg)
45
Monte Carlo (Particle Filter) Localization
![Page 46: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/46.jpg)
46
Monte Carlo (Particle Filter) Localization
1. Algorithm sample_normal_distribution(b):
2. return
1. Algorithm sample_triangular_distribution(b):
2. return
![Page 47: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/47.jpg)
47
Monte Carlo (Particle Filter) Localization
![Page 48: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/48.jpg)
48
Monte Carlo (Particle Filter) Localization
![Page 49: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/49.jpg)
49
Monte Carlo (Particle Filter) Localization
![Page 50: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/50.jpg)
50
Monte Carlo (Particle Filter) Localization
![Page 51: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/51.jpg)
51
![Page 52: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/52.jpg)
52
![Page 53: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/53.jpg)
53
![Page 54: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/54.jpg)
54
![Page 55: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/55.jpg)
55
![Page 56: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/56.jpg)
56
![Page 57: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/57.jpg)
57
![Page 58: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/58.jpg)
58
![Page 59: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/59.jpg)
59
![Page 60: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/60.jpg)
60
![Page 61: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/61.jpg)
61
![Page 62: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/62.jpg)
62
![Page 63: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/63.jpg)
63
![Page 64: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/64.jpg)
64
![Page 65: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/65.jpg)
65
![Page 66: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/66.jpg)
66
Monte Carlo (Particle Filter) Localization
![Page 67: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/67.jpg)
67
Monte Carlo (Particle Filter) Localization
![Page 68: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/68.jpg)
68
Multi-hypothesisTracking
![Page 69: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/69.jpg)
69
• Belief is represented by multiple hypotheses
• Each hypothesis is tracked by a Kalman filter
• Additional problems:
• Data association: Which observation
corresponds to which hypothesis?
• Hypothesis management: When to add / delete
hypotheses?
• Huge body of literature on target tracking, motion
correspondence etc.
Localization With MHT
![Page 70: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/70.jpg)
70
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
![Page 71: Probabilistic Robotics](https://reader034.fdocuments.us/reader034/viewer/2022051218/568158c7550346895dc6102f/html5/thumbnails/71.jpg)
71
MHT: Implemented System (3)Example run
Map and trajectory
# hypotheses
#hypotheses vs. time
P(Hbest)
Courtesy of P. Jensfelt and S. Kristensen