Landmark Map L11: Landmark Mapping Locations and uncertainties of - PDF document

Landmark Map L11: Landmark Mapping • 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 CS 344R/393R: Robotics Benjamin Kuipers • Problem : how to combine new information with old to update the map. A Spatial Relationship is a Vector Rotation Matrix • A spatial relationship holds between two � � � � � � cos � � sin � x G � = x B poses: the position and orientation of one, in � � � � � sin � cos � y G y B � � � � � � the frame of reference of the other. � x AB � Inverse: B � � x AB = y AB � cos � sin � � � � � � � sin � cos � � � � � � AB � � G T � [ ] x AB = x AB y AB � AB A B Uncertain Spatial Relationships Relation to Affine Transformations ˆ x = E [ x ] • The spatial T x AB = x AB [ y AB ] � AB ) T ] relationship C(x) = E [ (x � ˆ )(x � ˆ x x � � cos � AB � sin � AB x AB • is equivalent to • An uncertain spatial relationship is described � � the affine by a probability distribution of vectors, with a sin � AB cos � AB y AB � � transformation mean and a covariance matrix . � 0 0 1 � � � ˆ � x � � 2 � � x � xy � x � � � � � • Rotation plus ˆ ˆ 2 x = y C(x) = � xy � y � y � � � � � translation ˆ � 2 � � � � x � � y � � � � � � � � 1

A Map with n Landmarks Example • Concatenate n vectors into one big state vector • The robot senses object #1. � x 1 � � x ˆ � 1 � � � � • The robot moves. x 2 x ˆ � � � � x = ˆ 2 x = � M � • The robot senses a different object #2. � M � � � � � x n ˆ � � x � � • Now the robot senses object #1 again. n • And one big 3 n × 3 n covariance matrix. • After each step, what does the robot know � L � C ( x 1 ) C ( x 1 , x 2 ) C ( x 1 , x n ) � � (in its landmark map) about each object, C ( x 2 , x 1 ) C ( x 2 ) L C ( x 2 , x n ) � � C ( x ) = including itself? � M M O M � � � C ( x n , x 1 ) C ( x n , x 2 ) L C ( x n ) � � Robot Senses Object #1 and Moves Robot Senses Object #2 Robot Senses Object #1 Again Updated Estimates After Constraint 2

Compounding Compounding T x AC = x AC [ y AC ] � AC T T [ ] [ ] x BC = x BC y BC � BC x BC = x BC y BC � BC T T x AB = x AB [ y AB ] x AB = x AB [ y AB ] � AB � AB Compounding Computing Covariance • Let x AB be the pose of object B in the frame • Consider the linear mapping y = Mx+b of reference of A. (Sometimes written B A .) y = Mx + b • Given x AB and x BC , calculate x AC . C(y) = C ( Mx + b ) = E [(Mx + b � (M ˆ x + b)) (Mx + b � (M ˆ x + b)) T ] � x BC cos � AB � y BC sin � AB + x AB � � � )) T ] = E [M(x � ˆ x ) (M(x � ˆ x x AC = x AB � x BC = x BC sin � AB + y BC cos � AB + y AB � � ) T M T ] = E [M (x � ˆ )(x � ˆ x x � � � AB + � BC � � ) T ]M T = M E [(x � ˆ )(x � ˆ x x = MC(x)M T • Compute C ( x AC ) from C ( x AB ), C ( x BC ), and • Apply this to nonlinear functions by using C ( x AB, x BC ). Taylor Series. The Inverse Relationship Inverse Relationship • Let x AB be the pose of object B in the frame of reference of A. • Given x AB , calculate x BA . � � x AB cos � AB � y AB sin � AB � � � x BA = ( � ) x AB = x AB sin � AB � y AB cos � AB � � � � � AB � � � T x AB = x AB [ y AB ] � AB T [ ] x BA = x BA y BA � BA • Compute C ( x BA ) from C ( x AB ) 3

Composite Relationships Merging Information • Compounding combines relationships head- • An uncertain observation of a pose is to-tail: x AC = x AB ⊕ x BC combined with previous knowledge using • Tail-to-tail combinations come from the extended Kalman filter. observing two things from the same point: � , � x k P k – Previous knowledge: x BC = (  x AB ) ⊕ x AC – New observation: z k , R • Head-to-head combinations come from two observations of the same thing: • Update: x = x (new) ⊗ x (old) x AC = x AB ⊕ (  x CB ) • They provide new relationships between their endpoints. • Can integrate dynamics as well. Reversing and Compounding EKF Update Equations • OBJ1 R = (  ROBOT W ) ⊕ OBJ1 W � = f (ˆ x ˆ x k � 1 , u k ) • Predictor step: k • = WORLD R ⊕ OBJ1 W � = AP k � 1 A T + Q P k T ( HP k T + R ) � H � H � 1 • Kalman gain: K k = P k � + K k ( z k � h (ˆ � )) x ˆ k = ˆ x x • Corrector step: k k � P k = (I � K k H)P k Sensing Object #2 Observing Object #1 Again • OBJ2 W = ROBOT W ⊕ OBJ2 R • OBJ1 W = ROBOT W ⊕ OBJ1 R 4

Combining Observations (1) Combining Observations (2) • OJB1 W = OJB1 W (new) ⊗ OBJ1 W (old) • ROBOT W (new) = OJB1 W ⊕ (  OBJ1 R ) • OJB1 R = OJB1 R (new) ⊗ OBJ1 R (old) • ROBOT W = ROBOT W (new) ⊗ ROBOT W (old) Useful for Feature-Based Maps • We’ll see this again when we study FastSLAM. 5