Transcript ppt
Lecture 13:
Mapping Landmarks
CS 344R: Robotics
Benjamin Kuipers
Landmark Map
• Locations and uncertainties of n landmarks,
with respect to a specific frame of
reference.
– World frame: fixed origin point
– Robot frame: origin at the robot
• Problem: how to combine new information
with old to update the map.
A Spatial Relationship is a Vector
• A spatial relationship holds between two
poses: the position and orientation of one, in
the frame of reference of the other.
B
x AB
x AB
y AB
AB
x AB x AB
A
y AB
AB
T
Uncertain Spatial Relationships
xˆ E[x]
T
ˆ
ˆ
C(x) E[(x x)(x x) ]
• An uncertain spatial relationship is described
by a probability distribution of vectors, with a
mean and a covariance matrix.
xˆ
xˆ yˆ
ˆ
x2
C(x) xy
x
xy x
2
y y
2
y
A Map with n Landmarks
• Concatenate n vectors into one big state vector
x1
x 2
x
x n
xˆ 1
ˆ
x 2
xˆ
ˆ
x n
• And one big 3n3n covariance matrix.
C(x1 )
C(x1,x 2 )
C(x 2 ,x1 )
C(x 2 )
C(x)
C(x n ,x1 ) C(x n ,x 2 )
C(x1,x n )
C(x 2 ,x n )
C(x n )
Example
•
•
•
•
The robot senses object #1.
The robot moves.
The robot senses a different object #2.
Now the robot senses object #1 again.
• After each step, what does the robot know
(in its landmark map) about each object,
including itself?
Robot Senses Object #1 and Moves
Robot Senses Object #2
Robot Senses Object #1 Again
Updated Estimates After Constraint
Compounding
x BC x BC
x AB x AB
y AB
AB
T
y BC
BC
T
Compounding
x AC x AC
y AC
AC
T
x BC x BC
x AB x AB
y AB
AB
T
y BC
BC
T
Rotation Matrix
cos sin xG x B
sin cos yG y B
G
B
Compounding
• Let xAB be the pose of object B in the frame
of reference of A.
(Sometimes written BA.)
• Given xAB and xBC, calculate xAC.
x AC x AB x BC
x BC cos AB y BC sin AB x AB
x BC sin AB y BC cos AB y AB
AB BC
• Compute C(xAC) from C(xAB), C(xBC), and
C(xAB,xBC).
Computing Covariance
• Consider the linear mapping y = Mx+b
y Mx b
C(y) C(Mx b)
E[(Mx b (Mxˆ b)) (Mx b (Mxˆ b)) T ]
E[M(x xˆ ) (M(x xˆ )) T ]
E[M (x xˆ )(x xˆ ) T M T ]
M E[(x xˆ )(x xˆ ) T ]M T
MC(x) M T
• Apply this to nonlinear functions by using
Taylor Series.
Inverse Relationship
x AB x AB
y AB
AB
T
x BA x BA
y BA
BA
T
The Inverse Relationship
• Let xAB be the pose of object B in the frame
of reference of A.
• Given xAB, calculate xBA.
x BA ()x AB
x AB cos AB y AB sin AB
x AB sin AB y AB cos AB
AB
• Compute C(xBA) from C(xAB)
Composite Relationships
• Compounding combines relationships headto-tail: xAC = xAB xBC
• Tail-to-tail combinations come from
observing two things from the same point:
xBC = (xAB) xAC
• Head-to-head combinations come from two
observations of the same thing:
xAC = xAB (xCB)
• They provide new relationships between
their endpoints.
Merging Information
• An uncertain observation of a pose is
combined with previous knowledge using
the extended Kalman filter.
k
k
– Previous knowledge: x , P
– New observation: zk, R
• Update: x =
x(new) x(old)
• Can integrate dynamics as well.
EKF Update Equations
• Predictor step:
ˆxk f (xˆ k 1 ,uk )
k
P APk 1A Q
T
k
k
1
• Kalman gain: K k P H (HP H R)
• Corrector step:
T
T
ˆx k xˆ k K k (z k h(xˆ k ))
k
Pk (I K kH) P
Reversing and Compounding
• OBJ1R = (ROBOTW) OBJ1W
•
= WORLDR OBJ1W
Sensing Object #2
• OBJ2W = ROBOTW OBJ2R
Observing Object #1 Again
• OBJ1W = ROBOTW OBJ1R
Combining Observations (1)
• OJB1W = OJB1W(new) OBJ1W(old)
• OJB1R = OJB1R(new) OBJ1R(old)
Combining Observations (2)
• ROBOTW(new) = OJB1W (OBJ1R)
• ROBOTW = ROBOTW(new) ROBOTW(old)
Useful for Feature-Based Maps
• We’ll see this again when we study
FastSLAM.