Robot Motion and EKF-Localization
Download
Report
Transcript Robot Motion and EKF-Localization
Advanced Mobile Robotics
Probabilistic Robotics:
Motion Model/EKF Localization
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College
[email protected]
City College of New York
1
Robot Motion
• Robot motion is inherently uncertain.
• How can we model this uncertainty?
City College of New York
2
Bayes Filter Revisit
Bel ( xt ) P( zt | xt ) P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
• Prediction (Action)
bel ( xt ) p ( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
• Correction (Measurement)
bel( xt ) p( zt | xt ) bel( xt )
City College of New York
Probabilistic Motion Models
• To implement the Bayes Filter, we need
the transition model p(xt | xt-1, u).
• The term p(xt | xt-1, u) specifies a posterior
probability, that action u carries the robot
from xt-1 to xt.
• In this section we will specify, how
p(xt | xt-1, u) can be modeled based on the
motion equations.
City College of New York
4
Coordinate Systems
• In general the configuration of a robot can be described
by six parameters.
• Three-dimensional cartesian coordinates plus three
Euler angles pitch, roll, and tilt.
• Throughout this section, we consider robots operating
on a planar surface.
• The state space of such
systems is three-dimensional
(x, y, ).
City College of New York
5
Typical Motion Models
• In practice, one often finds two types of motion
models:
– Odometry-based
– Velocity-based (dead reckoning)
• Odometry-based models are used when systems
are equipped with wheel encoders.
• Velocity-based models have to be applied when
no wheel encoders are given.
• They calculate the new pose based on the
velocities and the time elapsed.
City College of New York
6
Example Wheel Encoders
These modules require +5V
and GND to power them,
and provide a 0 to 5V output.
They provide +5V output
when they "see" white, and a
0V output when they "see"
black.
These disks are manufactured out
of high quality laminated color
plastic to offer a very crisp black
to white transition. This enables a
wheel encoder sensor to easily see
the transitions.
Source: http://www.active-robots.com/
City College of New York
7
Dead Reckoning
• Derived from “deduced reckoning.”
• Mathematical procedure for determining the present
location of a vehicle.
• Achieved by calculating the current pose of the
vehicle based on its velocities and the time elapsed.
• Odometry tends to be more accurate than velocity
model,
• But, Odometry is only available after executing a
motion command, cannot be used for motion
planning
City College of New York
8
Reasons for Motion Errors
different wheel
diameters
ideal case
bump
carpet
and many more …
City College of New York
9
Odometry Model
• Robot moves from
x , y ,
to
x ' , y ' , '
.
u rot 1, rot
. 2 , trans
• Odometry information
trans ( x ' x ) 2 ( y ' y ) 2
rot 1 atan2( y' y, x ' x )
rot 2
rot 2 ' rot 1
x , y ,
rot 1
trans
x ' , y ' , '
Relative motion information, “rotation” “translation” “rotation”
City College of New York
The atan2 Function
• Extends the inverse tangent and correctly copes with the signs of x and y.
City College of New York
11
Noise Model for Odometry
• The measured motion is given by the true
motion corrupted with independent noise.
ˆrot 1 rot 1 |
ro t1 | 2
ˆtrans trans
| tra n s| 4 | ro t1 ro t2 |
1
3
ˆrot 2 rot 2
| tra n s|
1 | ro t2 | 2
City College of New York
| tra n s|
Typical Distributions for Probabilistic
Motion Models
Normal distribution
( x)
2
1
2
Triangular distribution
2
e
2
1x
2 2
0 if | x | 6 2
2 ( x) 6 2 | x |
6 2
City College of New York
Calculating the Probability (zero-centered)
• For a normal distribution
1.
Algorithm prob_normal_distribution(a, b):
2.
return
• For a triangular distribution
1.
Algorithm prob_triangular_distribution(a,b):
2.
return
City College of New York
14
Calculating the Posterior
An initial pose X
Given xt, xt-1, and u
A hypothesized final pose X
t-1
1.
2.
3.
4.
t
A pair of poses u obtained from odometry
Algorithm motion_model_odometry (xt, xt-1, u)
u xt 1
trans ( x ' x ) ( y ' y )
rot 1 atan2( y' y, x ' x )
rot 2 ' rot 1
2
2
xt
T
odometry values (u)
xt 1 ( x y )T
xt ( x y )T
10.
ˆtrans ( x' x) 2 ( y ' y ) 2
values of interest (xt-1, xt)
ˆrot 1 atan2( y' y, x'x)
ˆrot 2 ' ˆrot 1
p1 prob( rot1 ˆrot1,1 | ˆrot1 | 2ˆtrans )
p2 prob( trans ˆtrans ,3ˆtrans 4 (| ˆrot1 | | ˆrot2 |))
p prob( ˆ , | ˆ | ˆ )
11.
return p1 · p2 · p3
5.
6.
7.
8.
9.
3
rot 2
rot 2
1
rot 2
p( xt ut , xt 1 )
2 trans
prob(a, b)
Implements an error distribution over a
with zero mean and standard deviation b
City College of New York
15
Application
• Repeated application of the sensor model for
short movements.
• Typical banana-shaped distributions obtained for
2d-projection of 3d posterior.
p(xt| u, xt-1)
x’
u
x’
u
Posterior distributions of the robot’s pose upon executing the motion command
illustrated by the solid line. The darker a location, the more likely it is.
City College of New York
Velocity-Based Model
v
control u
City College of New York
Rotation r v
radius
17
Equation for the Velocity Model
Instantaneous center of curvature (ICC) at (xc , yc)
T
Initial pose xt 1 x y
x xc r sin
y yc r cos
Keeping constant speed, after ∆t time interval,
ideal robot will be at x x y T
t
x xc r sin( t )
x r sin r sin( t )
y y c r cos( t ) y r cos r cos( t )
t
t
City College of New York
Corrected,
-90
18
Velocity-based Motion Model
With xt 1
vˆt
vˆt
sin sin( ˆ t t )
ˆ t
x ' x ˆ t
' vˆt
vˆt
y
y
cos
cos( ˆ t t )
ˆ t
' ˆ t
ˆ t t
T
x y and xt x ' y ' ' Tare the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity vˆ t and a rotational velocity
T
Motion Control ut (vt t ) with additive Gaussian noise
( v ) 2
M t
vˆt vt (1 vt 2 t ) 2 vt
(0, M t )
ˆ t t 3 vt 4 t 2 t
Circular motion assumption leads to degeneracy ,
2 noise variables v and w 3D pose
Assume robot rotates ˆ when arrives at its final pose
City College of New York
1
t
2
0
t
ˆ t
0
( 3 vt 4 t ) 2
ˆt t
ˆ 5 v 6
19
Velocity-based Motion Model
Motion Model:
vˆt
vˆt
ˆ
sin
sin(
t
)
t
'
ˆ
ˆ
t
x x t
' vˆ
vˆt
t
ˆ
y
y
cos
cos(
t
)
t
ˆ
ˆ
'
t
t
ˆ t t ˆt
vˆt vt (1 vt 2 t ) 2
ˆ t t 3 vt 4 t 2
ˆ
5
v 6
1 to 4 are robot-specific error parameters
determining the velocity control noise
5 and 6 are robot-specific error parameters determining the
standard deviation of the additional rotational noise
City College of New York
20
Probabilistic Motion Model
How to compute p( xt ut , xt 1 ) ?
Center of circle:
Move with a fixed velocity during ∆t
resulting in a circular trajectory from
T
T
xt 1 x y to xt x y
with
Radius of the circle:
r * ( x x * ) 2 ( y y * ) 2 ( x x * ) 2 ( y y * ) 2
Change of heading direction:
a tan2( y y * , x x* ) a tan2( y y * , x x* )
dist r *
vˆ
t
t
ˆ
ˆ
t
t
City College of New York
(angle of the final rotation)
21
Posterior Probability for Velocity Model
Center of circle
Radius of the circle
Change of heading direction
Motion error: verr ,werr and ˆ
City College of New York
22
Examples (velocity based)
City College of New York
Map-Consistent Motion Model
p( x | u, x' )
p( x | u, x' , m)
Obstacle grown
by robot radius
Map free estimate
of motion model
p( x | u , x' )
“consistency” of
pose in the map
p( x | m)
“=0” when placed
in an occupied cell
Approximation: p( x | u, x' , m) p( x | m) p( x | u, x' )
City College of New York
Summary
• We discussed motion models for odometry-based and
velocity-based systems
• We discussed ways to calculate the posterior probability
p(x| x’, u).
• Typically the calculations are done in fixed time intervals
t.
• In practice, the parameters of the models have to be
learned.
• We also discussed an extended motion model that takes
the map into account.
City College of New York
25
Localization, Where am I?
?
• Given
– Map of the environment.
– Sequence of measurements/motions.
• Wanted
– Estimate of the robot’s position.
• Problem classes
– Position tracking (initial robot pose is known)
– Global localization (initial robot pose is unknown)
– Kidnapped robot problem (recovery)
City College of New York
26
Markov Localization
Markov Localization: The straightforward application of Bayes filters to the localization problem
City College of New York
27
Bayes Filter Revisit
Bel ( xt ) P( zt | xt ) P( xt | ut , xt 1 ) Bel ( xt 1 ) dxt 1
• Prediction (Action)
bel ( xt ) p ( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
• Correction (Measurement)
bel( xt ) p( zt | xt ) bel( xt )
City College of New York
EKF Linearization
First Order Taylor Expansion
• Prediction:
g (ut , t 1 )
g (ut , xt 1 ) g (ut , t 1 )
( xt 1 t 1 )
xt 1
g (ut , xt 1 ) g (ut , t 1 ) Gt ( xt 1 t 1 )
• Correction:
h( t )
h( xt ) h( t )
( xt t )
xt
h( xt ) h( t ) H t ( xt t )
City College of New York
29
EKF Algorithm
1.
Extended_Kalman_filter( t-1, St-1, ut, zt):
2.
3.
4.
Prediction:
t g (ut , t 1 )
5.
6.
7.
8.
Correction:
Kt St HtT (Ht St HtT Qt )1
t t Kt ( zt h(t ))
9.
Return t, St
t At t 1 Bt ut
St Gt St 1GtT Rt
St At St 1 AtT Rt
St ( I Kt Ht )St
h( t )
Ht
xt
City College of New York
Kt St CtT (Ct St CtT Qt )1
t t Kt ( zt Ct t )
St (I Kt Ct )St
g (ut , t 1 )
Gt
xt 1
30
1.
EKF_localization ( t-1, St-1, ut, zt, m):
Prediction:
3. Gt
g (ut , t 1 )
xt 1
x'
t 1, x
y '
t 1, x
'
t 1, x
g (ut , t 1 )
ut
x'
vt
y '
vt
'
v
t
5.
Vt
6.
1 | vt | 2 | t |2
M t
0
x'
t 1, y
y '
t 1, y
'
t 1, y
x'
t 1,
y '
Jacobian of g w.r.t location
t 1,
'
t 1,
x'
t
y '
t
'
t
2
3 | vt | 4 | t |
t g (ut , t 1 )
8. St Gt St 1GtT Vt MtVtT
0
7.
City College of New York
Jacobian of g w.r.t control
Motion noise covariance
Matrix from the control
Predicted mean
Predicted covariance
31
Velocity-based Motion Model
With xt 1 x y
vˆt
vˆt
sin sin( ˆ t t )
ˆ t
x ' x ˆ t
' vˆt
vˆt
y y cos cos( ˆ t t )
ˆ t
' ˆ t
ˆ t t
T and xt x ' y ' ' Tare the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity
vˆ t and a rotational velocity ˆ t
T
Motion Control ut (vt t ) with additive Gaussian noise
vˆt vt (1 vt 2 t ) 2 vt
(0, M t )
ˆ t t 3 vt 4 t 2 t
( 1 vt 2 t ) 2
M t
0
City College of New York
0
( 3 vt 4 t ) 2
32
Velocity-based Motion Model
Motion Model:
vt
vt
sin
sin(
t
)
t
'
t
x x t
' vt
vt
y y cos cos( t t ) N (0, Rt )
t
' t
t t
xt g (ut , xt 1 ) N (0, Rt )
g (ut , t 1 )
g (u t , xt 1 ) g (u t , t 1 )
( xt 1 t 1 )
xt 1
g (u t , xt 1 ) g (u t , t 1 ) Gt ( xt 1 t 1 )
City College of New York
33
Velocity-based Motion Model
v
vt
sin t sin( t t )
t
x ' x t
' vt
v
y y cos t cos( t t ) N (0, Rt )
t
' t
t
t
x '
t 1, x
g (u t , t 1 ) y '
Gt ( xt 1 , t 1 )
xt 1
t 1, x
t 1, x
x '
t 1, y
y
t 1, y
t 1, y
x '
t 1,
y
t 1,
t 1,
City College of New York
x
Derivative of g
along x’ dimension,
w.r.t. x at t 1
t 1, x
Jacobian of g w.r.t location
34
Velocity-based Motion Model
v
Mapping between the motion noise in control
vt
sin t sin( t t )
t
space to the motion noise in state space
x ' x t
' vt
v
y y cos t cos( t t ) N (0, Rt )
t
' t
t
t
x' x'
Jacobian of g w.r.t control
vt t
y ' y '
g (ut , t 1 )
Vt
Derivative of g w.r.t. the motion parameters,
ut
v
t
t
evaluated at u t and t 1
' '
v
t
t
vt (sin sin( t t )) vt cos( t t )t
sin sin( t t )
2
t
t
t
cos cos( t t )
v (cos cos( t t )) vt sin( t t )t
t
2
t
t
t
t
0
St Gt St 1GtT Vt MtVtT
City College of New York
35
1.
EKF_localization ( t-1, St-1, ut, zt, m):
Correction:
3.
zˆt
2
2
mx t , x my t , y
atan2m , m
y
t,y
x
t,x
t ,
h( t , m)
xt
5.
Ht
6.
r2 0
Qt
2
0 r
rt
t,x
t
t , x
rt
t , y
t
t , y
Predicted measurement mean
rt
t ,
t
t ,
Jacobian of h w.r.t location
7.
St Ht St HtT Qt
Pred. measurement covariance
8.
Kt St HtT St1
Kalman gain
9.
t t Kt ( zt zˆt )
Updated mean
10. St I Kt Ht St
Updated covariance
City College of New York
36
Feature-Based Measurement Model
•
Jacobian of h w.r.t location
2
rti (m j , x x) 2 (m j , y y ) 2
r
i
t a tan 2(m j , y y, m j , x x) ) 2
s i
s
j
t
s2
zti h( xt , j, m) N (0, Qt )
h( t )
h( xt ) h( t )
( xt t )
xt
Ht
h( t , m)
xt
rt
t,x
t
t , x
rt
t , y
t
t , y
rt
t ,
t
t ,
j Cti Is the landmark that corresponds to the measurement of
City College of New York
zti
37
EKF Localization
with known
correspondences
City College of New York
38
EKF Localization
with unknown
correspondences
Maximum likelihood estimator
City College of New York
39
EKF Prediction Step
City College of New York
40
EKF Observation Prediction Step
City College of New York
41
EKF Correction Step
City College of New York
42
Estimation Sequence (1)
City College of New York
43
Estimation Sequence (2)
City College of New York
44
Comparison to Ground Truth
City College of New York
45
UKF Localization
?
• Given
– Map of the environment.
– Sequence of measurements/motions.
• Wanted
– Estimate of the robot’s position.
• UKF localization
City College of New York
46
Unscented Transform
Sigma points
Weights
w
0
i
0
m
( n )S
i
n
wmi wci
w
0
c
1
2(n )
n
(1 2 )
for i 1,...,2n
Pass sigma points through nonlinear function
i g( i )
For n-dimensional Gaussian
λ is scaling parameter that determine how far
the sigma points are spread from the mean
If the distribution is an exact Gaussian, β=2 is
the optimal choice.
Recover mean and covariance
2n
' wmi i
i 0
2n
S' wci ( i )( i ) T
i 0
City College of New York
47
UKF_localization ( t-1, St-1, ut, zt, m):
Prediction:
1 | vt | 2 | t |2
M t
0
2
3 | vt | 4 | t |
0
Motion noise
r2 0
Qt
2
0
r
Measurement noise
ta1 tT1
Augmented state mean
S t 1
S ta1 0
0
0 0T 0 0T
0
Mt
0
0
0
Qt
Augmented covariance
ta1 ta1 ta1 Sta1
tx g ut tu , tx1
ta1 Sta1
Sigma points
Prediction of sigma points
2L
t wmi ix,t
i 0
2L
Predicted mean
St w t t
i 0
i
c
x
i ,t
x
i ,t
T
Predicted covariance
City College of New York
48
UKF_localization ( t-1, St-1, ut, zt, m):
Correction:
t htx tz
Measurement sigma points
2L
zˆt wmi i ,t
Predicted measurement mean
i 0
2L
St w i ,t zˆt i ,t zˆt
i 0
S
i
c
2L
x, z
t
T
w t i ,t zˆt
i 0
i
c
Kt Stx, z St
x
i ,t
1
Pred. measurement covariance
T
Cross-covariance
Kalman gain
t t Kt ( zt zˆt )
Updated mean
St St Kt St K
Updated covariance
T
t
City College of New York
49
UKF Prediction Step
City College of New York
50
UKF Observation Prediction Step
City College of New York
51
UKF Correction Step
City College of New York
52
EKF Correction Step
City College of New York
53
Estimation Sequence
EKF
PF
UKF
City College of New York
54
Estimation Sequence
EKF
UKF
City College of New York
55
Prediction Quality
EKF
UKF
City College of New York
56
Kalman Filter-based System
• [Arras et al. 98]:
• Laser range-finder and vision
• High precision (<1cm accuracy)
[Courtesy of Kai Arras]
City College of New York
57
Multihypothesis
Tracking
City College of New York
58
Localization With MHT
• 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.
City College of New York
59
MHT: Implemented System (1)
• Hypotheses are extracted from Laser Range Finder (LRF) scans
• Each hypothesis has probability of being the correct one:
Hi { xˆ i, Si , P (Hi )}
• Hypothesis probability is computed using Bayes’ rule
P( s | H i ) P( H i )
P( H i | s)
P( s)
• Hypotheses with low probability are deleted.
• New candidates are extracted from LRF scans.
C j {z j , Rj }
City College of New York
[Jensfelt et al. ’00]
60
MHT: Implemented System (2)
Courtesy of P. Jensfelt and S. Kristensen
City College of New York
61
MHT: Implemented System (3)
Example run
# hypotheses
P(Hbest)
Map and trajectory
Courtesy of P. Jensfelt and S. Kristensen
#hypotheses vs. time
City College of New York
62
Thank You
63
City College of New York