Mapping Topics: Topological maps SLAM HMMs Revisited Additional reading B J. Kuipers&Y.-T. exploration and mapping based on a semantic hierarchy of JJ. Leonard. I. J. Cox Int J. Robotics Resear hyte. "Dynamic map building for an autonomous mobile robot Ssues Statement Given a set of observations of the world what is the world like? Inputs Sequence of observations, or perhaps actions and observations, or perhaps actions observations and vehicle motion/sensor models Outputs from different algorithms Graph of connected states Graph of connected states, transition probabilities p(xi x, ak) and observation probabilities p(zxi) Description of landmark locations Occupancy grid List of points from range sensors Choices: How to represent model <Closing the loop
Mapping Topics: Topological Maps SLAM HMMs Revisited ● Additional reading: ● B. J. Kuipers & Y.-T. Byun. 1991. “A robot exploration and mapping strategy based on a semantic hierarchy of spatial representations.” Journal of Robotics and Autonomous Systems, 8: 47-63 ● J. J. Leonard, I. J. Cox, and H. F. Durrant-Whyte. “Dynamic map building for an autonomous mobile robot”. Int. J. Robotics Research, 11(4):286--298, August 1992. Issues ● Statement: – Given a set of observations of the world, what is the world like? ● Inputs: – Sequence of observations, or perhaps actions and observations, or perhaps actions, observations and vehicle motion/sensor models ● Outputs from different algorithms: – Graph of connected states – Graph of connected states, transition probabilities p(xi |xj ,ak) and observation probabilities p(zi |xj ) – Description of landmark locations – Occupancy grid – List of points from range sensors ● Choices: – How to represent model “Closing the loop
Graphical Models Actions Bell p(xlap Observations Observable 2 hidden state Want to estimate p(Xi x, ax), p(zixi)instead of 区X1x2,…,Xx Topological Maps Idea: build map as graph of sensor signatures and control laws connecting Extracted Topology nodes Actions are control laws Observations are .? p()={0,1} Metric map layered on top Layer geometric information on topology
Graphical Models States x1 x2 p(xj |ai , xi ) Z2 b Beliefs 1 Observations Z1 a Actions 1 p(zj |xi ) b2 Z2 Hidden Observable ● Want to estimate p(xi |xj ,ak), p(zi |xj ) instead of {x1,x2,...,xT} Topological Maps ● Idea: build map as graph of sensor signatures and control laws connecting nodes ● Actions are control laws ● Observations are...? ● p(•|•) = {0, 1} ● Layer geometric information on topology Extracted Topology Metric map layered on top
Building the Topology Assume a set of distinctiveness measures 1. dn and a set of local control strategies Choose a lcs 2. Follow lcs until maximum in one of d is seen 3. Hill-climb on d, until maximum is reached 4. Move towards open space 5. Repeat A simple environment The Distinctiveness surface Hill-Climbing 易 D P2777 WALL2 WALL3 WALL2 WALL3 C Measures and strategies Measures Range variance Lateral range variance Range symmetry Temporal discontinuity Number of directions of reasonable motion Temporal change in number of directions of motion Strategies Follow the midline Move along Object on Left Move along Object on Right Blind step
Building the Topology ● Assume a set of distinctiveness measures, d1,...,dn and a set of local control strategies 1. Choose a LCS 2. Follow LCS until maximum in one of di is seen 3. Hill-climb on di until maximum is reached 4. Move towards open space 5. Repeat A simple environment The Distinctiveness Surface Hill-Climbing Measures and Strategies ● Measures: – Range variance – Lateral range variance – Range symmetry – Temporal discontinuity – Number of directions of reasonable motion – Temporal change in number of directions of motion ● Strategies – Follow the midline – Move along Object on Left – Move along Object on Right – Blind step C A B D E P1 P2 WALL2 WALL2 WALL3 WALL1 WALL3
Pros and cons Human-like maps Exploration strategy built into mapping process Purely on-line Con: Loop closure Will not necessarily map space completely Absence of decision -theoretic measures means no measure of map quality a large number of free parameters and heuristics Purely on-line The Extended Kalman Filter for Building Maps Process model is now x=f(x1-1,l1-1,q=1) Measurement model is h(x1,r;) Linearising, we get x,=f(x1-1,1-1,0) x≈x1+A(x1-1-x1)+Wq1 二≈h(x1,0)+H(x1-x,)+V1 Where A.H. w and v are the jacobians ah ch afr, a af, ah
Pros and Cons ● Pro:● Human-like maps ● Exploration strategy built into mapping process ● Purely on-line ● Con:● Loop closure ● Will not necessarily map space completely ● Absence of decision-theoretic measures means no measure of map quality ● A large number of free parameters and heuristics ● Purely on-line ● Process model is now ● Measurement model is ● Linearising, we get ● where A, H, W and V are the Jacobians: The Extended Kalman Filter for Building Maps ( , , ) t = t−1 t−1 t−1 x f x u q ( , ) t t t z = h x r t t t t t t t t t t t t t z h x H x x Vr x x A x x Wq x f x u ≈ + − + ≈ + − + = − − − − − ) ~ ,0) ( ~( ( ˆ ) ~ ( , ,0) ~ 1 1 1 1 1 ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = n n n n x f x f x f x f A L M O M L 1 1 1 1 ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = n m m n x h x h x h x h H L M O M L 1 1 1 1 ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = n n n n q f q f q f q f W L M O M L 1 1 1 1 ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ ∂ ∂ ∂ ∂ ∂ ∂ ∂ ∂ = m m m m v h v h v h v h V L M O M L 1 1 1 1
Updates for the eKF Still using only first two moments Process model f(i CI=ACIA +w,ew Measurement model K,=CH,(HCIH+V, V) x,=x,+K,(=1-h(x,0) C1=(-K,H,)C1 Leonard. Cox and Durranf-White /992 Robot mapping Initial belief Action Measurement △ t cose+ gAt coS6 +△sinb+q△sin 6+A6+qA6 5=0 Mx △b h(,v)= range .-x)+(2,-y bearing tan
Updates for the EKF ● Still using only first two moments. ● Process model: ● Measurement model: T t t t T t t t t t t t C AC A W QW x f x u = + = − − − − − 1 1 1 ˆ (ˆ , ,0) − − − − − − = − = + − = + t t t t t t t t t T t t t T t t t T t t t C I K H C x x K z h x K C H H C H V RV ( ) ˆ ˆ ( (ˆ ,0)) ( ) 1 Robot Mapping Initial belief Action Measurement ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = y x y x 1 1 λ λ ξ θ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + ∆ + ∆ + ∆ + ∆ + ∆ + ∆ = x x q y t q t x t q t f u q 1 1 sin sin cos cos ( , , ) λ λ θ θ θ θ θ θ θ ξ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ ∆ ∆ = θ t u ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = b r z ( ) ( ) ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎣ ⎡ − + ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛ − − − + − + =⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = − θ θ λ λ λ λ ξ v x y x y v h v x y x y r 1 2 2 tan ( , ) bearing range ξ t−1 − ξt ξ t ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ = y x λ λ λ Leonard, Cox and Durrant-White, 1992