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.