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

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

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

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?

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?

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?

Core Problem Motion planning with both collision and visibility constraints

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

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

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

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)

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

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

Point Acquisition

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

From Points to Polylines

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

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

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

Computed Safe Spaces normal exaggerated

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

Merging of Four Partial Models

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

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

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

Next-Best View Computation

NBV Example 1 (Simulation)

NBV Example 2 (Simulation)

Comparison

Map Building

3D Model Construction

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

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

Target-Finding Strategy

Animated Target-Finding Strategy

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

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

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

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

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

Effect of Geometry on # of Robots Two robots are needed

Critical Curve

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

Example of Cell Decomposition

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

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

Example of Target-Finding Strategy Visible Clear Contaminated

Visible Clear Contaminated

More Complex Example with No Hole

Example with Recontaminations

Linear # of Recontaminations Recontaminated area

Example with Two Robots (Greedy algorithm)

Example with Two Robots (Greedy algorithm)

Example with Three Robots (Greedy algorithm)

Extension: Robot with Cone of Vision

III. Target Tracking

Pure Visual Servoing Target Observer Observer’s visual field

A Better Strategy Target Observer

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

Tracking a Fast Target

Best Placement? Target Observer

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

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

Multiple Targets And Observers

Target Tracking without Planner

Target Tracking with Planner

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