Intro to Robotics, Handout –Lecture 22:
Name:
BFS: algorithm that computes distance from s to all points in 𝓠𝐟𝐫𝐞𝐞
Queue Obstacle array
BFS(s, obstacle) 1. _____
1. set all Distances to 2. _____
2. Distance(s) = 3. _____
4. _____
3. Queue(1) = 5. _____
4. Current = 6. _____
5. Empty = 7. _____
6. while Empty > Current 8. _____
1. v = 9. _____
2. for each neighbor n of : 10. _____
11. _____ Distance array
1. If ( n is not and 12. _____
Distance(n) > ) 13. _____
1. Distance(n) = 14. _____
2. Queue(Empty) = 15. _____
3. Empty = 16. _____
3. Current = 17. _____
18. _____
7. Return ____________
19. _____
1. Top example: s = 8 Queue Obstacle array
1. _____
2. Bottom example: s =6 2. _____
3. _____
4. _____
5. _____
6. _____
7. _____
8. _____
9. _____
10. _____ Distance array
11. _____
12. _____
13. _____
14. _____
15. _____
16. _____
17. _____
18. _____
19. _____
Page 1 of 4
A search algorithm that incrementally explores 𝓠𝐟𝐫𝐞𝐞 while searching for a path
𝑈 = artificial _______________________
𝑈(𝑞) = _____________________________
Solution to find 𝑞𝑠 is _____________________________________
𝜏(𝑞) = −𝛻𝑈(𝑞) = _______________________________
Difficulties
1. Hard to construct ___________________________________________________
2. Harder to compute the ________________________________________________
Why? Computing shortest distances to __________________________________, ________________________
Solution: Workspace potential fields to ________________________________________________________
Attractive Field
Conic well potential
𝑈𝑎𝑡𝑡,𝑖 (𝑞) = ________________________________
𝐹𝑎𝑡𝑡,𝑖 (𝑞) = _____________________________
Let 𝑜𝑖 (𝑞𝑓 ) = 1, Draw 𝑈𝑎𝑡𝑡,𝑖 (𝑞) and gradient:
Problem: gradient has _________________________________
__________________________________________
Parabolic Well potential:
𝑈𝑎𝑡𝑡,𝑖 (𝑞) = ____________________________
𝐹𝑎𝑡𝑡,𝑖 (𝑞) = _____________________________
Let 𝑜𝑖 (𝑞𝑓 ) = 1, Draw 𝑈𝑎𝑡𝑡,𝑖 (𝑞) and gradient:
Problem: gradient force converges ____________________________________
Page 2 of 4
Combination Well potential:
1 2
𝜁𝑖 ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ ≤ 𝑑
𝑈𝑎𝑡𝑡,𝑖 (𝑞) = { 2
1
𝑑𝜁𝑖 ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ − 𝜁𝑖 𝑑 2 ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ > 𝑑
2
−𝜁𝑖 (𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )) ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ ≤ 𝑑
𝐹𝑎𝑡𝑡,𝑖 (𝑞) = {
−Sign (𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )) ‖𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 )‖ > 𝑑
Let 𝑜𝑖 (𝑞𝑓 ) = 1, Draw 𝑈𝑎𝑡𝑡,𝑖 (𝑞) and gradient. Let d = 1, 𝜁 = 1.
Repulsive force should:
1 1 1
𝜂 ( − ) 𝜌(𝑜𝑖 (𝑞)) ≤ 𝜌0
𝑈𝑟𝑒𝑝,𝑖 (𝑞) = { 2 𝜌(𝑜𝑖 (𝑞)) 𝜌0
0 𝜌(𝑜𝑖 (𝑞)) > 𝜌0
1 1 1
𝐹𝑟𝑒𝑝,𝑖 (𝑞) = −𝛻𝑈𝑟𝑒𝑝,𝑖 (𝑞) = −𝜂𝑖 ( − ) 2 ∇𝜌(𝑜𝑖 (𝑞))
𝜌(𝑜𝑖 (𝑞)) 𝜌0 𝜌 (𝑜𝑖 (𝑞))
Let 𝜌0 = 1, and have an obstacle at 1.
Draw 𝑈𝑟𝑒𝑝,𝑖 (𝑞) and gradient. Let 𝜂 = 1.
Page 3 of 4
Intro to Robotics
Class Worksheet – Preparation for Lecture 23
1. Download the Matlab code for potential fields, PotentialFieldNavigation.m. Set the
parameters so it does not converge. Print a screen shot.
https://www.mathworks.com/matlabcentral/fileexchange/73617-robot-arm-potential-
field-navigation (or www.mathworks.com/matlabcentral/fileexchange/73617 )
2. Verify 𝐹𝑎𝑡𝑡,𝑖 (𝑞) = −𝛻𝑈𝑎𝑡𝑡 (𝑞) = −𝜁𝑖 (𝑜𝑖 (𝑞) − 𝑜𝑖 (𝑞𝑓 ))
1 1 1
3. Verify 𝐹𝑟𝑒𝑝_𝑖 (𝑞) = −𝛻𝑈𝑟𝑒𝑝,𝑖 (𝑞) = −𝜂𝑖 (𝜌(𝑜 (𝑞)) − 𝜌 ) 𝜌2(𝑜 (𝑞)) ∇𝜌(𝑜𝑖 (𝑞))
𝑖 0 𝑖
Page 4 of 4
