Why listen to me?
CMP3103M/CMP9050M– Lecture 6
Navigation 1 – Where am I going?
Dr. Mike Mangan
Course Information: Syllabus
Date Title Lecturer
23/01/2017 Intro Marc Hanheide
30/01/2017 Robot Programming Marc Hanheide
06/02/2017 Robot Sensing Marc Hanheide
13/02/2017 Motion & Control Marc Hanheide
20/02/2017 Robot Behaviour Mike Mangan
27/02/2017 Navigation Mike Mangan
06/03/2017 Navigation Mike Mangan
13/03/2017 Robot mapping – SLAM Mike Mangan
20/03/2017 Control Architectures Paul Baxter
27/03/2017 HRI 1 Paul Baxter
03/04/2017 HRI 2 Paul Baxter
24/04/2017 Applications Paul Baxter
01/05/2017 Revision / Robot Systems Marc Hanheide
What do we mean by navigation?
Navigation (mass noun)
The process or activity of accurately ascertaining one’s position and planning and following a route.
Guidance (mass noun)
The directing of the motion or position of something, especially an aircraft, spacecraft, or missile.
This week…
1. Beaconing (when you can see the goal)
2. Visual Homing (when you can see the local surroundings)
3. Path Planning (when you have a map)
4. Search strategies (when you have nothing)
This week…
1. Beaconing (when you can see the goal)
2. Visual Homing (when you can see the local surroundings)
3. Path Planning (when you have a map)
4. Search strategies (when you have nothing)
1. Beaconing
Starting with the simplest
Landmark Navigation / Taxis / Beaconing
Directed movement towards an observable cue
Passive
Starting with the simplest
Active
Landmark Navigation / Taxis / Beaconing
Directed movement towards an observable cue
Reactive Strategies Often Sufficient
Input
(sensor)
Output
(motor)
Positive Transfer function
Negative Transfer function
Reactive Strategies Often Sufficient
Aggressive
Approach
Limitations
1. Cues must be visible
a) Temporarily obscured
b) Short range
2. Where complex planning is needed
Limitations
1. Cues must be visible
a) Temporarily obscured
b) Short range
2. Where complex planning is needed
2. Visual Homing
<- Target 2. Visual Homing Nico Tinbergen showed in the 1920s that wasps approach their hidden nests using surrounding visual cues e.g. the pattern of surrounding objects. Similar behaviours found many animals: • bees • ants • humming birds • rats 2. Visual Homing – snapshot model 1. Before leaving home 1. take a visual “snapshot” 2. When homing 1. Move to reduce the mismatch in landmark pattern between current view and memory 2. Move and repeat Nest Cartwright & Collett, 1982 2. Visual Homing – snapshot model 1. Before leaving home 1. take a visual “snapshot” 2. When homing 1. Move to reduce the mismatch in landmark pattern between current view and memory 2. Move and repeat 3. Catchment area defines the region from which you can home Nest Cartwright & Collett, 1982 2. Visual Homing – snapshot model Problems: 1. How to define a landmark? 2. How to match landmarks between current scene and memory? (correspondence problem) 3. You must be allinged 2. Visual Homing – ALV model ALV = Average Landmark Vector 1. Before leaving home 1. take a visual “snapshot” 2. Point a unit vector to each “landmark” 3. Take the average and store 2. When homing 1. Compute the average landmark vector as above 2. Use a vector subtraction to generate the home vector Lambrinos et al, 1998 [0,-1] [1.4,1.4][-1.4,1.4] 𝐴𝐿𝑉 = −1.4+0+1.4 3 , 1.4−1+1.4 3 𝐴𝐿𝑉 = 0,0.6 𝐻𝑜𝑚𝑒 𝑉𝑒𝑐𝑡𝑜𝑟 = 𝐴𝐿𝑉𝑐𝑢𝑟𝑟𝑒𝑛𝑡 − 𝐴𝐿𝑉ℎ𝑜𝑚𝑒 2. Visual Homing – ALV model ALV = Average Landmark Vector 1. Before leaving home 1. take a visual “snapshot” 2. Point a unit vector to each “landmark” 3. Take the average and store 2. When homing 1. Compute the average landmark vector as above 2. Use a vector subtraction to generate the home vector [0,0.6] 𝐴𝐿𝑉 = −1.4+0+1.4 3 , 1.4−1+1.4 3 𝐴𝐿𝑉 = 0,0.6 Lambrinos et al, 1998 𝐻𝑜𝑚𝑒 𝑉𝑒𝑐𝑡𝑜𝑟 = 𝐴𝐿𝑉𝑐𝑢𝑟𝑟𝑒𝑛𝑡 − 𝐴𝐿𝑉ℎ𝑜𝑚𝑒 2. Visual Homing – ALV model ALV = Average Landmark Vector 1. Before leaving home 1. take a visual “snapshot” 2. Point a unit vector to each “landmark” 3. Take the average and store 2. When homing 1. Compute the average landmark vector as above 2. Use a vector subtraction to generate the home vector [0,0.6] Lambrinos et al, 1998 [0,-1] [-0.7,-0.7] [0.7,-0.7] 𝐴𝐿𝑉 = −0.7+0+0.7 3 , −0.7−1−0.7 3 𝐴𝐿𝑉 = 0,−0.9 𝐻𝑜𝑚𝑒 𝑉𝑒𝑐𝑡𝑜𝑟 = 𝐴𝐿𝑉𝑐𝑢𝑟𝑟𝑒𝑛𝑡 − 𝐴𝐿𝑉ℎ𝑜𝑚𝑒 2. Visual Homing – ALV model ALV = Average Landmark Vector 1. Before leaving home 1. take a visual “snapshot” 2. Point a unit vector to each “landmark” 3. Take the average and store 2. When homing 1. Compute the average landmark vector as above 2. Use a vector subtraction to generate the home vector [0,0.6] Lambrinos et al, 1998 𝐻𝑜𝑚𝑒 𝑣𝑒𝑐𝑡𝑜𝑟 = (0-0,-0.9-0.6) 𝐻𝑜𝑚𝑒 𝑣𝑒𝑐𝑡𝑜𝑟 = 0,−1.5 𝐻𝑜𝑚𝑒 𝑉𝑒𝑐𝑡𝑜𝑟 = 𝐴𝐿𝑉𝑐𝑢𝑟𝑟𝑒𝑛𝑡 − 𝐴𝐿𝑉ℎ𝑜𝑚𝑒 [0,-0.9] 2. Visual Homing – ALV Problems: 1. How to define a landmark? 2. How to match landmarks between current scene and memory? (correspondence problem) 3. You must be allinged Home Reading: Variant called COMALV can also do away with landmark extraction and might be applicable to other sensory modalities. See Hafner, 2001 2. Visual Homing Methods - Problems: 1. How to define a landmark? 2. How to match landmarks between current scene and memory? (correspondence problem) 3. Images must be rotationally alinged Zeil, 2003 2. Visual Homing Methods - Problems: 1. How to define a landmark? 2. How to match landmarks between current scene and memory? (correspondence problem) 3. Images must be rotationally alinged Zeil (2003) showed that the pixelwise difference (e.g. RMS, SSD) between home and current images increases with distance giving a gradient that can be followed to return home. 2. Visual Homing Methods - Successes: Reproducing animal behaviour: Ants - Sahabot (Lambrinos et al) Crickets – (Mangan & Webb, 2008) Robot vacuum cleaners: Moller Group, Univ Bielefeld Image Warping 2. Visual Homing Methods - Successes: Reproducing animal behaviour: Ants - Sahabot (Lambrinos et al) Crickets – (Mangan & Webb, 2008) Robot vacuum cleaners: Moller Group, Univ Bielefeld Image Warping 2. Visual Homing Methods - Successes: Reproducing animal behaviour: Ants - Sahabot (Lambrinos et al) Crickets – (Mangan & Webb, 2008) Robot vacuum cleaners: Moller Group, Univ Bielefeld Image Warping 2. Where would you use visual homing? Long-range line-of-sight provides long distance guidance Final approach to specific location (automated piloting) Short line-of-sight requires many memories which are easy aliased (more on this next week!) 3. Path Planning 3. Path Planning S TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path F 1 11 1 11 1 11 1 11 1 1 11 1 What is the minimum-cost path? 5 3. Path Planning S TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path G 1 11 1 11 1 11 1 11 1 1 11 1 3. Path Planning S TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path Brute Force search too costly - Requires an algorithm Today we will look at: - Dijkstra's algorithm - A* algortihm G 1 11 1 11 1 11 1 11 1 1 11 1 3. Path Planning TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path Brute Force search too costly - Requires an algorithm Today we will look at: - Dijkstra's algorithm - A* algortihm 3. Path Planning TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path Brute Force search too costly - Requires an algorithm Today we will look at: - Dijkstra's algorithm - A* algortihm 3. Path Planning TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path Brute Force search too costly - Requires an algorithm Today we will look at: - Dijkstra's algorithm - A* algortihm 3. Path Planning S TASK: Plan the path from Start (S) to Finish (F) that minimises the total cost. Should take into account known obstacles to your path Brute Force search too costly - requires an algorithm Today we will look at: - Dijkstra's algorithm - A* algorithm G 1 11 1 11 1 11 1 11 1 1 11 1 Path Planning Example S F A C E B D 1 2 4 1 2 2 2 2 Path Planning Example S F A C E B D 1 2 4 1 2 2 2 2 Dijkstra’s Algorithm 1. Setup Phase 1. Set start node as current node with distance 0 2. Set distance to all other start node from start node to infinity (∞) 3. \ S F A C E B D 1 2 4 1 2 2 2 V S A B C D E F 2 Nodes Visited Nodes Dijkstra’s Algorithm 1. Setup Phase 1. Set start node as current node with distance 0 2. Set distance to all other start node from start node to infinity (∞) 3. \ S F A C E B D 1 2 4 1 2 2 2 V S A B C D E F S 0S 2 Dijkstra’s Algorithm 1. Setup Phase 1. Set start node as current node with distance 0 2. Set distance to all other start node from start node to infinity (∞) 3. \ S F A C E B D 1 2 4 1 2 2 2 V S A B C D E F S 0S ∞ ∞ ∞ ∞ ∞ ∞ 2 Dijkstra’s Algorithm 2 Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length V S A B C D E F S 0S ∞ ∞ ∞ ∞ ∞ ∞ S F A C E B D 1 2 4 1 2 2 2 2 0+1 0+2 Dijkstra’s Algorithm 2 Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ S F A C E B D 1 2 4 1 2 2 2 2 Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 2 S F A C E B D 1 2 4 1 2 2 2 2 Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to univisted node with smallest total path length V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S ∞ ∞ ∞ ∞ 2 S F A C E B D 1 2 4 1 2 2 2 2 Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ 2 S F A C E B D 1 2 4 1 2 2 2 2 Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ C Now you try! Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ C 0S 1S 2S 3A 4B 4C 7C Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ C 0S 1S 2S 3A 4B 4C 7C D Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ C 0S 1S 2S 3A 4B 4C 7C D 0S 1S 2S 3A 4B 4C 7C Dijkstra’s Algorithm Repeat until all nodes have been visited 1. Update table with total distance (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to unvisited node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 1S 2S ∞ ∞ ∞ ∞ A 0S 1S 2S 3A ∞ ∞ ∞ B 0S 1S 2S 3A 4B ∞ ∞ C 0S 1S 2S 3A 4B 4C 7C D 0S 1S 2S 3A 4B 4C 7C E 0S 1S 2S 3A 4B 4C 6E Dijkstra’s Algorithm – Limitations 1. Will flood search densely and homogeneously connected nodes. 1. Undirected Wiki has a great resource with demos & psuedocode https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm Dijkstra’s Algorithm – Limitations 1. Will flood search densely and homogeneously connected nodes. 1. Undirected 2 S F A C E B D 1 2 4 1 2 2 2 2 Wiki has a great resource with demos & psuedocode https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm Questions? A* Algorithm – Overview Nilsson et al adapted Dijkstra’s algorithm to improve performance and implemented on Shakey the robot in 1968. The resultant approach is A* search. A* selects its distance of travel by optimising 𝑓 𝑛 = 𝑔 𝑛 + ℎ(𝑛) where: n is the current node g(n) is the cost of the path from the start node to n h(n) is a heuristic that estimates the cost of the cheapest path from n to the goal. Common heuristic is straight line distance to goal but is problem specific https://en.wikipedia.org/wiki/Heuristic A* Algorithm – Setup the same 1. Setup Phase 1. Set start node as current node with distance 0 2. Set distance to all other start node from start node to infinity (∞) 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S ∞ ∞ ∞ ∞ ∞ ∞ h(A)=10 h(B)=6 h(C)=6 h(D)=4 h(F)=0 h(E)=2 A* Algorithm – Update using cost function Repeat until all cannot expand goal node 1. Update table with total cost (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 11S 8S ∞ ∞ ∞ ∞ 𝑓 𝑛 = 𝑔 𝑛 + ℎ(𝑛) h(A)=10 h(B)=6 h(C)=6 h(D)=4 h(F)=0 h(E)=2 A* Algorithm – Update using cost function Repeat until all cannot expand goal node 1. Update table with total cost (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 11S 8S ∞ ∞ ∞ ∞ B 𝑓 𝑛 = 𝑔 𝑛 + ℎ(𝑛) h(A)=10 h(B)=6 h(C)=6 h(D)=4 h(F)=0 h(E)=2 Now you try! A* Algorithm – Update using cost function Repeat until all cannot expand goal node 1. Update table with total cost (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 11S 8S ∞ ∞ ∞ ∞ B 0S 11S 8S ∞ 8B ∞ ∞ D 0S 11S 8S ∞ 8B 8D ∞ E 0S 11S 8S ∞ 8B 8D 8E 𝑓 𝑛 = 𝑔 𝑛 + ℎ(𝑛) h(A)=10 h(B)=6 h(C)=6 h(D)=4 h(F)=0 h(E)=2 A* Algorithm – Update using cost function Repeat until all cannot expand goal node 1. Update table with total cost (e.g. from start) from current node to all connected, unvisited nodes. 2. Update path length and previous node if less than current setting 3. Move to node with smallest total path length 2 S F A C E B D 1 2 4 1 2 2 2 2 V S A B C D E F S 0S 11S 8S ∞ ∞ ∞ ∞ B 0S 11S 8S ∞ 8B ∞ ∞ D 0S 11S 8S ∞ 8B 8D ∞ E 0S 11S 8S ∞ 8B 8D 8E 𝑓 𝑛 = 𝑔 𝑛 + ℎ(𝑛) h(A)=10 h(B)=6 h(C)=6 h(D)=4 h(F)=0 h(E)=2 A* Algorithm – from the designer https://www.youtube.com/watch?v=mQ7M-zhiu7U Nils Nilson Wiki has a great resource with demos & psuedocode https://en.wikipedia.org/wiki/A*_search_algorithm 4. Search strategies Spiral Search Brownian Motion Continuous search commonly with small changes in step size but random turns. Observed in particles and some animals searching for local resources. https://en.wikipedia.org/wiki/Brownian_ motion Levy Flights Levy flights use a long-tailed distribution to select the step size generating clusters of searches interspersed with long paths into new areas Observed in many animals including bees & sharks. https://seeingcomplexity.wordpress.com/ 2011/02/16/sharks-the-sp-500-and-levy- flights/ step size frequency Levy Flights Levy flights use a long-tailed distribution to select the step size generating clusters of searches interspersed with long paths into new areas Observed in many animals including bees & sharks. https://seeingcomplexity.wordpress.com/ 2011/02/16/sharks-the-sp-500-and-levy- flights/ • Cartwright, B. A., and T. S. Collett. "How honey bees use landmarks to guide." Nature 295 (1982): 561. • Lambrinos, Dimitrios, et al. "Landmark navigation without snapshots: the average landmark vector model." Proc. Neurobiol. Conf. Göttingen. 1998. • Hafner, Verena Vanessa. "Adaptive homing—robotic exploration tours." Adaptive Behavior 9.3-4 (2001): 131- 141. • Mangan, Michael, and Barbara Webb. "Modelling place memory in crickets." Biological cybernetics 101.4 (2009): 307-323. References Recommended Reading • Ulrich Nehmzow, “Mobile Robotics: A Practical Introduction”, Springer, 2nd edition, 2002. • Bekey, G.A., Autonomous Robots: From biological inspiration to implementation and control, (MIT Press). Chapter 14. Questions? Workshops & Assessment 1. All cut off dates for demonstrating robot capabilities extended by 1 week • Week 5 – tasks 3&4 • Week 6 – task 4 2. Visual object search 1. 70% of 30% for practical 2. Individual 3. Assessment by implementation submission plus presentation (w10,11) Disclaimers: • subject to change • Test environment will be different