1. Motion Planning with Visibility Constraints Jean-Claude Latombe Computer Science Department Stanford University

2. Main Collaborators u Hector Gonzalez u Steve LaValle u David Lin u Eric Mao u T.M. Murali u Leo Guibas u Cheng-Yu Lee u Rafael Murrieta

3. Autonomous Observer Mobile robot that performs visual data-collection tasks autonomously in complex environments, e.g.: Mobile robot that performs visual data-collection tasks autonomously in complex environments, e.g.: –Construct a map/model of an environment –Find and track a moving target among obstacles

4. Map Building u A robot or a team of robots is introduced in an unknown environment u Where should the robot(s) successively go in order to build a map/model of the environment as efficiently as possible?

5. Target Finding u An evasive target hides in an environment with obstacles u A map of the environment is available u How should a robot or a team of robots move to sweep the building and eventually find the target?

6. Target Tracking u An evasive target is initially in the field of view of a robot, but may escape behind an obstacle. u A map of the environment is available. u How should the robot or the team of robots move to keep the target in the field of view of at least one robot at each time?

7. Core Problem Motion planning with both collision and visibility constraints

8. Some Applications u Intelligent camera u Telepresence, cooperation of geographically dispersed groups u 4D modeling of buildings u Automatic inspection of large structures u Architectural/archeological modeling u Surveillance and monitoring of plants u Military scouting

9. I. Map Building u Goal: Efficiently build a polygonal layout of an indoor environment u Question: Where should the robot go to perform the next sensing operation? u Approach: Randomized Next-Best View (NBV) motion planning algorithm

10. Why a Polygonal Layout? u Convenient to compute navigation paths and to extract topological information u Allows visibility computation u Compact model facilitating data exchanges, such as wireless communication among robots u Possibility of inserting uncertainty information

11. Robot Hardware u Nomadic Super-Scout wheeled platform u Sick laser range sensor mounted horizontally u 30 scans/s (180-dg scan, 360 pt/scan)

12. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polylines –Align P with p –Compute the safe space f corresponding to p –Compute the new safe space as the union of F and f

13. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polyline –Align P with p –Compute the safe space f corresponding to p –Compute the new safe space as the union of F and f

14. Point Acquisition

15. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polylines –Align P with p –Compute the safe space f corresponding to p –Compute the new safe space as the union of F and f

16. From Points to Polylines

17. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polylines –Align P with p –Compute the safe space f corresponding to p –Compute the new safe space as the union of F and f

18. Alignment of Two Sets of Polylines u Pick two edges from smallest set at random u Match against two edges with same angle in other set u Evaluate quality of fit

19. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polylines –Align P with p –Compute the safe space f corresponding to p –Compute the new safe space as the union of F and f

20. Computed Safe Spaces normal exaggerated

21. Construction of 2D Layouts u Go to successive sensing positions q 1, q 2, …, until the safe space F has no free edge u At each position q k, let M = (P,F) be the partial layout constructed so far. Do: –Acquire a list L of points –Transform L into a set p of polylines –Compute the safe space f corresponding to p –Align P with p –Merge M and (p,f) by taking the union of F and f

22. Merging of Four Partial Models

23. Side-Effects of Merging Technique Detection, elimination, and separate recording of: –Transient objects –Small objects

24. Dealing with Small Objects u Detect spikes in the safe space f u Record the “apex” -- a small edge segment -- into a separate small-object map

25. Next-Best View Algorithm u Let M=(P,F) be the current partial layout. u Pick many points q i at random in F u Discard every q i if length of edges in P visible from q i is below a certain threshold (for reliable alignment) u Measure goodness of each q i as function of both amount of new space potentially visible from q i (through free edges) and length of shortest path (within F, avoiding small obstacles) from robot’s current position to q i u Select best q i as next sensing position

26. Next-Best View Computation

27. NBV Example 1 (Simulation)

28. NBV Example 2 (Simulation)

29. Comparison

30. Map Building

31. 3D Model Construction

32. II. Target Finding u Goal: Find a target that is hiding in an environment cluttered with obstacles u Questions: How many robots are needed? How they should sweep the environment? u Approach: Cell decomposition of an information state

33. Assumptions u Target is unpredictable and can move arbitrarily fast u Environment is polygonal, with or without holes u Target and robots are modeled as point objects u A robot finds the target when the line segment connecting them does not intersect any obstacles

34. Target-Finding Strategy

35. Animated Target-Finding Strategy

36. Related Work u Art-Gallery problems: O’Rourke, 1987; many others u Pursuit-evasion games: Isaacs, 1965; Hajek, 1975; Basar and Olsder, 1982; many others u Pursuit-evasion in a graph: Parsons, 1976; Megiddo et al., 1988; Lapaugh, 1993 u Pursuit-evasion in a simple polygon: Suzuki and Yamashita, 1992

37. Results u Lower bounds on the minimum number of robots N needed in environment with n edges and h holes u NP -hardness of computing N in given environment u Complete planner in environments searchable by one robot. Planner is rather fast in practice, but its worst-case running time is exponential in n u Greedy algorithm for environments requiring multiple robots. But no guarantee of optimality for number of robots u Extensions: cone of vision, aerial robot

38. Effect of Number n of Edges Minimal number of robots  log n)

39. Effect of Number n of Edges Minimal number of robots  log n)

40. Effect of Number h of Holes  sqrt(h))

41. Effect of Geometry on # of Robots Two robots are needed

42. Critical Curve

43. Conservative Cells In each conservative cell the set of visible edges remain constant

44. Example of Cell Decomposition

45. Information State Example of an information state = (1,1,0) 0 : the target does not hide beyond the edge 1 : the target may hide beyond the edge

46. Search Graph u {Nodes} = {Conservative Cells} X {Information States} u Node (c,i) is connected to (c’,i’) iff: –Cells c and c’ share an edge (i.e., are adjacent) –Moving from c, with state i, into c’ yields state i’ u Initial node (c,i) is such that: –c is the cell where the robot is initially located –i = (1, 1, …, 1) u Goal node is any node where the information state is (0, 0, …, 0) Size is exponential in the number of edges in workspace

47. Example of Target-Finding Strategy Visible Clear Contaminated

48. Visible Clear Contaminated

49. More Complex Example with No Hole

50. Example with Recontaminations

51. Linear # of Recontaminations Recontaminated area

52. Example with Two Robots (Greedy algorithm)

53. Example with Two Robots (Greedy algorithm)

54. Example with Three Robots (Greedy algorithm)

55. Extension: Robot with Cone of Vision

56. III. Target Tracking

57. Pure Visual Servoing Target Observer Observer’s visual field

58. A Better Strategy Target Observer

59. Example of Target-Tracking Motions No distance constraint Constant distance between robot and target target robot

60. Tracking a Fast Target

61. Best Placement? Target Observer

62. Minimum Time to Escape (MTE) u To compute MTE: –Visibility region –Shortest path –(5ms) u Goal: maximize MTE –Randomized strategy –(100ms) Target Observer

63. Adaptability u Depth of view limitations u View angle u Holonomic/non- holonomic

64. Multiple Targets And Observers

65. Target Tracking without Planner

66. Target Tracking with Planner

69. Conclusion u Motion planning with visibility constraints raises various problems combining geometry and control, ranging from theoretical to applied and experimental u Close relations with art-gallery problems, but with moving guards u No single technical approach so far: random sampling, cell decomposition u Important future extensions: 3D maps, better visibility models