Path planning and source
detection
in unknown environments
Richard Tsai
University of Texas at Austin
Joint work with Martin Burger, Yana Landa, and Nick Tanushev
Research supported by NSF, Sloan Foundation, and ARO
Exploration/surveillance
Shortest path to see a smelly object
Epoisse
Shortest path to see a smelly object
Do it without a map?
Epoisse
Major components
•
•
Computing visibility (skipped in this talk)
•
•
φ(x, O)
when a map is available
without a map
Path planning
•
•
•
map out and inspect the domain
discover and visually inspect the source(s)
do it “well” -- optimization, adaptivity
Defining visibility
Obstacle:
Γ = ∂Ω = ∂{Ψ < 0}
Observing location: O
Construct a continuous function
φ(y, O)
{y : φ(y, O) < 0} = occlusion from O
φ(x, y) = φ(y, x)
(reciprocity)
Related work
• Computing visibility:
• Algorithms for handling surfaces:
• Geometry: convexity and topology of the illuminated
region of a solid surface [Ghomi]
• Path planning:
•
Hamilton-Jacobi, computational geometry, boundary,
medial axes approaches. [LaValle][Landa-Tsai]
Source detection:
Inverse source problems
[Yamamoto][Santosa-Symes][Isakov] and many others
Obstacles reconstructed by an ENO technique.
Creating a map
Level set representation of the environment.
Simulation with real data
Convergence theory
• Single observer
• Convex objects
• Separation: non-overlapping disks, non-overlapping inscribing triangles
• Finite number of objects
Convergence
C1
Convergence
C1
Convergence
C1
Some statistics
2
Sample environment
300
Number of steps histogram:
approach nearest edge
300
Number of steps histogram:
random walk
1
0
−1
−2
−2
200
200
100
100
0
−1
0
x
1
2
100
200
300
400
0
0
100
200
300
400
1000 independent runs
Shortest path to see an object
Target’s GPS coordinates given.
Known obstacle configuration.
S
O
Shortest path to see an object
u(x,O): visibility from O
reciprocity:
u(x,y)=u(y,x)
Compute what S can see: u(x,S)
Compute a geodesics
from O to the shadow boundary of S
S
O
Shortest path to see multiple objects
• Extension to multiple observers
• Algorithm determines the existence of solutions.
Target detection in unknown environments
•Obstacles in the domain are unknown
•Given the location of the target: S
•Visibility from observing location available
Move around to see the target (S) as soon as possible.
2
y
1
0
!1
!2
!2
!1
0
x
1
2
•Obstacles in the domain are unknown
•Given the location of the target: S
•Visibility from observing location available
2
2
1
1
0
0
y
y
Move around to see the target (S) as soon as possible.
!1
!2
!2
!1
!1
0
x
1
2
!2
!2
!1
0
x
1
2
Shortest path if obstacles are given.
2
1
1
0
0
y
y
2
!1
!2
!2
!1
!1
0
x
1
2
!2
!2
!1
0
x
1
2
Shortest path when obstacles are known.
Where is the cheese?
O
S?
Info available:
• Obstacles
• visibility φ(·, O)
• smell: u(O, S), ∇1 u(O, S)
(intensity, gradient)
Reciprocity
Self-adjoint problem
−∆u = δ(x − S),
(how the cheese smells)
u|∂Ω ≡ 0
Ω
S
O
Reciprocity
Self-adjoint problem
−∆u = δ(x − S),
u|∂Ω ≡ 0
−∆v = δ(x − O),
v|∂Ω ≡ 0
(how the cheese smells)
(how my cheese smells)
Ω
S
O
Reciprocity
Self-adjoint problem
−∆u = δ(x − S),
u|∂Ω ≡ 0
−∆v = δ(x − O),
v|∂Ω ≡ 0
(how the cheese smells)
(how my cheese smells)
−
!
Ω
∆u v dx = −
Ω
S
!
u ∆v dx
Ω
v(S) = u(O)
u(x, y)|y=S = u(x)
u(x, y) = u(y, x)
O
Search with complete knowledge of obstacles
Determine: source location (S)
−∆u = δ(x − S),
−∆v = δ(x − O),
1
S
0.8
u|∂Ω ≡ 0
v|∂Ω ≡ 0
0.6
0.4
v(S) = u(O) = I
0.2
0
given intensity
−0.2
−0.4
−0.6
O
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
=⇒ S ∈ {v = I}
1
1
0.8
0.8
0.6
0.6
0.4
0.4
0.2
0.2
0
0
−0.2
−0.2
−0.4
−0.4
−0.6
−0.6
−0.8
−0.8
−1
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
−1
−1
−0.5
0
0.5
1
Derivative information
∆w
(j)
∂
=
δ(x − O),
∂xj
=⇒ w
(j)
w
(j)
|∂Ω ≡ 0
(S) = uxj (O)
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
−0.6
−0.8
−1
−1
−0.5
0
0.5
1
Confidence function
uk := u(Ok ), pk := ux (Ok ), qk := uy (Ok )
hk (x)
−α(vk (x)−uk )2
:= e
(1)
+
−β(wk (x)−pk )2
c0 (e
(2)
−β(wk (x)−qk )2
+e
H(x) =
!
hk (x)
)
Partial visibility information on obstacles
?
S?
O
Info available:
• visibility: φ(·, O)
• smell: u(O, S), ∇1 u(O, S)
(intensity, gradient)
Adaptively increase the knowledge on the obstacle,
and source location
?
?
?
Comparison principle
−∆v = δ(x − O) in Ω , v|Ω ≡ 0
c
v ≥ 0 = v+
−∆v± = δ(x − O) in Ωc± , ṽ± |Ωc± ≡ 0
Ω− ⊂ Ω ⊂ Ω+
Ω−
=⇒ v− ≥ v ≥ v+
I = u(O) = v(S) ≤ v− (S)
≥ v+ (S)
i.e.
S ∈ {x : v− (x) ≥ I ≥ v+ (x)}
Ω+
S ∈ {x : v− (x) ≥ I ≥ v+ (x)}
Sandwiching by under and over approx. obstacles Ω− ⊂ Ω ⊂ Ω+
Search with increasing knowledge
of obstacles from vis. information
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
Search with increasing knowledge
of obstacles from vis. information
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
Search terminates when
the region of possible source location is visible
at current vantage location.
1
0.5
0
−0.5
−1
−1
−0.5
0
0.5
1
Search terminates when
the region of possible source location is visible
at current vantage location.
Single source of unknown strength
Complete knowledge of the obstacle.
−∆u = α δ(x − S),
−∆vj = δ(x − Oj ),
u|∂Ω ≡ 0
v|∂Ω ≡ 0,
j = 1, 2
O1 != O2
α v1 (S) = u(O1 )
α v2 (S) = u(O2 )
=⇒
v1 (x)
u(O1 )
S ∈ {x :
=
}
v2 (x)
u(O2 )
Single source of unknown strength
Partial knowledge of the obstacle.
−∆u = α δ(x − S),
−∆v± = δ(x − O) in
u|∂Ω ≡ 0
c
Ω± ,
ṽ± |Ωc± ≡ 0
Ω− ⊂ Ω ⊂ Ω+
=⇒ v− ≥ v ≥ v+ ≥ 0
α v− (S) ≥ u(O1 )
α v+ (S) ≤ u(O2 )
=⇒
v− (x)
u(O1 )
S ∈ {x :
≥
}
v+ (x)
u(O2 )
Where are the cheeses?
S?
−∆u =
!
δ(x − Sj ),
u|∂Ω ≡ 0
O
S?
Info available:
• Obstacles
• visibility φ(·, O)
• smell: u(O, S), ∇1 u(O, S)
(intensity, gradient)
Multiple sources and theirs decays
−∆uj = δ(x − Sj ),
u=
uj |∂Ω ≡ 0
|O − S1 | "| O − Sj |,
!
uj
j
j #= 1
u1 (O) ∼
!
uj (O)
1
7
0.8
0.6
6
0.4
5
0.2
4
0
3
−0.2
−0.4
2
−0.6
1
−0.8
−1
−1
0
−1
−0.5
0
0.5
1
−0.5
0
0.5
1
Multiple sources and theirs decays
−∆uj = δ(x − Sj ),
u=
uj |∂Ω ≡ 0
|O − S1 | "| O − Sj |,
!
uj
j
j #= 1
u1 (O) ∼
!
uj (O)
Effective source ~ one of the sources
1
7
0.8
0.6
6
0.4
5
0.2
4
0
3
−0.2
−0.4
2
−0.6
1
−0.8
−1
−1
0
−1
−0.5
0
0.5
1
−0.5
0
0.5
1
Inconsistent information from sensors
sum of all h
0.5
0
!0.5
h for v
!0.5
h for wx
!0.5
h for wy
!0.5
0.5
0.5
0.5
Consistent 0information from
sensors
0
!0.5
level lines of v wx vy
h for wx
h hV
for vhWx hWy
sum of all
v
wx
wy
4
0.5
2
00
!4
4
2
!0.5
h for
sum
of wy
all h
0.5
0.5
0.5
0
00
sum of all h
4
2
sum of all hV hWx hWy
!0.5
level lines of v wx!0.5
vy
!0.5
!2
sum of all hV hWx hWy
0
!0.5
!0.5
v
wx
wy
0.5
0
0
!2
level lines of v wx wy
!4
v
wx
wy
!0.5
sum of all h
0.5
0
0
!2
!0.5
!4
level lines of v wx wy
sum of all h
sum of all hV hWx hWy
4
2
v
wx
wy
0
0
0
!2
!
!4
level lines of v wx wy
sum of all h
sum of all hV hWx hWy
4
2
v
wx
wy
0
0
0
!2
!
!4
Identified souce: S̃1
−∆ũ1 = δ(x − S̃1 ),
ũj |∂Ω ≡ 0
level lines of v wx wy
sum of all h
sum of all hV hWx hWy
4
2
v
wx
wy
0
0
0
!2
!
!4
Identified souce: S̃1
−∆ũ1 = δ(x − S̃1 ),
Adjusted “sniffing”: S̃2 ∈ {v = I − ũ1 (O)}
ũj |∂Ω ≡ 0
I=
!
uk (O)
level lines of v wx wy
sum of all h
sum of all hV hWx hWy
v
wx
wy
4
2
0
0
0
!2
!
!4
−∆ũ1 = δ(x − S̃1 ),
Identified souce: S̃1
I=
Adjusted “sniffing”: S̃2 ∈ {v = I − ũ1 (O)}
Hint for new observing location.
(121($(341!$%&$2$5,$2$
!"#$%&$'(($)*$)+,$)+$
2
5,
5-
.
/
0
!/
$
!.
$
ũj |∂Ω ≡ 0
!
uk (O)
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
17&71"1897-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4
5
&
*+
*,
-./"$#"011"!
'()
6
'
'
!6
!5
!4
!'()
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
15&51"1675-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4()
4
&
*+
*,
-./"$#"011"!
'()
'()
'
'
!'()
!4
!4()
!'()
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
Hint for new observing location.
15&51"1675-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4()
4
&
*+
*,
-./"$#"011"!
'()
'()
'
'
!'()
!4
!4()
!'()
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
17&71"1897-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4
5
&
*+
*,
-./"$#"011"!
'()
6
'
'
!6
!5
!4
!'()
!"#$%&'(#)%*+")
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
18&81"19:8-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4
&
*+
*,
-./"$#"011"!
'(5
'(6
'()
'(7
'
'
!'()
!'(7
!4
!'(6
!'(5
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
Useless information from three random observing locations.
Effective source locations drastically different from the truth.
18&81"19:8-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4
&
*+
*,
-./"$#"011"!
'(5
'(6
'()
'(7
'
'
!'()
!'(7
!4
!'(6
!'(5
!"#$%"&
!"#$%"*+
!"#$%"*,
'()
'()
'()
'
'
'
!'()
!'()
!'()
Useless information from three random observing locations.
Effective source locations drastically different from the truth.
18&81"19:8-"$#"&"*+"&,
-./"$#"011"!2"!3+"!3,
4
&
*+
*,
-./"$#"011"!
'(5
'(6
'()
'(7
'
'
!'()
!'(7
!'(6
!4
!'(5
sum of all h
0.5
Result by a “smarter” sampling
0
!0.5
Discussion
•Algorithm applicable to other linear
problems (self-adjoint or not)
•Stability: bounds on the gradients of u
•Path strategy
•Multiple sources of different (unknown)
strengths
•Non-standard inverse problems
(fewer sensors, freedom in sensor locations,
two modalities)