Motion Planning with Visibility Constraints

1
Motion Planning with Visibility Constraints

• Jean-Claude Latombe
• Computer Science Department
• Stanford University

2
Main Collaborators

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

3
Autonomous Observer

• 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

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

5
Target Finding

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

6
Target Tracking

• An evasive target is initially in the field of
  view of a robot, but may escape behind an
  obstacle.
• A map of the environment is available.
• 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

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

9
I. Map Building

• GoalEfficiently build a polygonal layout of an
  indoor environment
• Question Where should the robot go to perform
  the next sensing operation?
• Approach Randomized Next-Best View (NBV) motion
  planning algorithm

10
Why a Polygonal Layout?

• Convenient to compute navigation paths and to
  extract topological information
• Allows visibility computation
• Compact model facilitating data exchanges, such
  as wireless communication among robots
• Possibility of inserting uncertainty information

11
Robot Hardware

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

12
Construction of 2D Layouts

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Pick two edges from smallest set at random
• Match against two edges with same angle in other
  set
• Evaluate quality of fit

19
Construction of 2D Layouts

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Go to successive sensing positions q1, q2, ,
  until the safe space F has no free edge
• At each position qk, 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

• Detect spikes in the safe space f
• Record the apex -- a small edge segment -- into
  a separate small-object map

25
Next-Best View Algorithm

• Let M(P,F) be the current partial layout.
• Pick many points qi at random in F
• Discard every qi if length of edges in P visible
  from qi is below a certain threshold (for
  reliable alignment)
• Measure goodness of each qi as function of both
  amount of new space potentially visible from qi
  (through free edges) and length of shortest path
  (within F, avoiding small obstacles) from robots
  current position to qi
• Select best qi 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

• GoalFind a target that is hiding in an
  environment cluttered with obstacles
• Questions How many robots are needed? How they
  should sweep the environment?
• Approach Cell decomposition of an information
  state

33
Assumptions

• Target is unpredictable and can move arbitrarily
  fast
• Environment is polygonal, with or without holes
• Target and robots are modeled as point objects
• 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

• Art-Gallery problems ORourke, 1987 many others
• Pursuit-evasion games Isaacs, 1965 Hajek, 1975
  Basar and Olsder, 1982 many others
• Pursuit-evasion in a graph Parsons, 1976
  Megiddo et al., 1988 Lapaugh, 1993
• Pursuit-evasion in a simple polygon Suzuki and
  Yamashita, 1992

37
Results

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

38
Effect of Number n of Edges
Minimal number of robots N O(log n)
39
Effect of Number n of Edges
Minimal number of robots N Q(log n)
40
Effect of Number h of Holes
N Q(sqrt(h))
41
Effect of Geometry on of Robots
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)
46
Search Graph

• Nodes Conservative Cells X Information
  States
• 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
• Initial node (c,i) is such that
• c is the cell where the robot is initially
  located
• i (1, 1, , 1)
• 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
48
Example of Target-Finding Strategy
49
More Complex Example with No Hole
50
Example with Recontaminations
51
Linear of Recontaminations
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
Observers visual field
Observer
58
A Better Strategy
Target
Observer
59
Example of Target-Tracking Motions
Constant distance between robot and target
No distance constraint
target
robot
60
Tracking a Fast Target
61
Best Placement?
Target
Observer
62
Minimum Time to Escape (MTE)

• To compute MTE
• Visibility region
• Shortest path
• (5ms)
• Goal maximize MTE
• Randomized strategy
• (100ms)

Target
Observer
63
Adaptability

• Depth of view limitations
• View angle
• Holonomic/non-holonomic

64
Multiple Targets And Observers
65
Target Tracking without Planner
66
Target Tracking with Planner
67
Target Tracking with Planner
68
Target Tracking with Planner
69
Conclusion

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