Information Gathering Control via Exploratory Path ... - UCLA Vision Lab
Information Gathering Control via Exploratory Path Planning Luca Valente§
Richard Y.-H. Tsai†
Stefano Soatto§
§ University of California, Los Angeles, http://vision.ucla.edu † University of Texas at Austin, [email protected]
Abstract—We tackle the problem of exploring an unknown environment using a range sensor, where most of the uncertainty is due to the presence of opaque objects of unknown and arbitrary geometry, topology and configuration. Information-gathering control aims to minimize the residual uncertainty, resulting in an optimal control problem that is essentially intractable. We propose a surrogate measure of uncertainty that is efficient to compute and results in guaranteed performance bounds. The resulting algorithm is proven to complete exploration within a prescribed accuracy in a finite number of steps, depending in the complexity of the environment. Index Terms—Path planning, unknown environment, exploration, motion control, visibility computation, 3d reconstruction, mapping, localization, slam.
I. I NTRODUCTION We consider a spatial domain represented by a subset of the real plane. Obstacles, or “objects,” are simply-connected subsets of the domain. An omni-directional range sensor (or “viewer”) is represented by a point on the domain, that can move continuously in the region not occupied by obstacles, and yields a measurement of the distance from the sensor to each visible obstacle. Given an unknown environment (domain and obstacle field) and a starting point x0 , we are interested in designing a path of finite length at the end of which the viewer will have “seen” all obstacles. This can be formalized as an optimal control problem with the resulting Hamilton-Jacobi-Bellmann partial differential equation (PDE), or (discrete) sequential decision process. In either case, the resulting optimization is intractable for all but the most trivial cases. Our goal is therefore to devise surrogates or heuristics that result in algorithms that are provably sufficient, in the sense of eventually discovering all the visible region of the domain, and possibly efficient, in the sense of doing so in a finite number of steps depending on the complexity or volume of the environment. Greedy exploratory strategies aim to minimize, at each step, the residual uncertainty about the environment, that is the conditional entropy of the “map” given the measurements. In our case, where the only uncertain is due to visibility, we can think of the map as a probability distribution that is uniform and equal to zero in the part of the domain that has been seen, and positive otherwise. The maximally informative strategy then corresponds to maximizing the discovered area. Therefore, efficient computation of visibility is an important component of an exploratory path planner. For a known environment, this problem has been studied extensively in computational geometry.
When obstacles are represented implicitly as the zero level set of a function, [6] one can determine visibility by comparing the geodesic and Euclidean distances between the observer x0 and a point x solving the Eikonal equation. If ψ is the signed distance function to the objects surfaces, fast sweeping methods [3] can be used: Following the notation of [9], let D be some bounded domain; for any x0 ∈ D, the visibility function from x0 is generated via φ(x) =
min
ψ(z), x ∈ D
z∈L(x0 ,x)
(1.1)
where L(x0 ,x) is the segment [x0 ,x], and φ can be computed by solving the PDE y−x y−x = min{H(φ − ψ)∇ψ · |y−x| ,0} ∇φ · |y−x| (1.2) φ(x0 ) = ψ(x0 ) if the environment is known [4]. When the obstacles are unknown, [8], [5], [11] proposed algorithms to construct piecewise linear paths so that any point on the surface of the obstacles is seen by at least one vertex of the path. Each continuous portion of the visible boundary terminates with the occluding boundaries, or “horizon points” or edges on the visibility map. When a new horizon appears, it is stored in a list, and removed once explored. The algorithm terminates when the list L is empty. It should be noted that occluding boundaries are detectable both with range sensors as well as with optical sensors under suitable conditions [1]. We introduce an exploration algorithm that is guaranteed to be sufficient, in the sense of discovering almost all the visible area in a finite number of steps, and efficient, in the sense of requiring a small number of re-planning steps that is bounded depending on the complexity of the environment. We prove the results for the case where objects are polygonal, although the algorithm does not make that assumption, and can handle obstacles of arbitrary shape. II. BACKGROUND AND NOTATION Obstacles, or “objects, are denoted by Ω ⊂ D and their surfaces (boundaries) by Γ = ∂Ω. The obstacles are perfectly light-absorbing and cannot be crossed by the observer. Note that Ω does not uniquely define ψ. A common choice is the signed distance to Γ, which can be computed efficiently [3]. Lemma 1: Assume that ψ is Lipschitz continuous on D, that is, there exists some K > 0 such that |ψ(x2 ) − ψ(x1 )| ≤ K|x2 − x1 | for any x1 ,x2 ∈ D. Then, for any y ∈ D, |φ(y,x2 ) − φ(y,x1 )| ≤ K|x2 − x1 |
Corollary 2.1: Since φ(x,y) = φ(y,x) for any x,y,∈ D, under the same assumptions as lemma 1, φ(·,x) is also KLipschitz on D for any x ∈ D. |φ(y2 ,x) − φ(y1 ,x)| ≤ K|y2 − y1 | for any y1 ,y2 ∈ D Theorem 2.2: (Rademacher, 1919) Let n and m denote two positive integers and U an open subset of Rn . If f : U → Rm is locally Lipschitz, then it is differentiable at almost every x ∈ U (with respect to the Lebesgue measure). A proof of this result can be found in [2]. We can immediately state the following corollary: Corollary 2.3: Assume that ψ is K-Lipschitz for some K > 0. Then for any y ∈ D, ∇x φ(y,·) is defined almost everywhere on D. When multiple objects are present, ψ can be expressed using a number p of level set functions (ψi )i=1,···,p . Lemma 2: Assume that the scene can be described with ψ(x) = minj=1,···,p ψj (x). For any j ∈ {1,··· ,p}, define φj (y,x) = mint∈[0,1] ψj (x + t(y,x)). ˜ Finally, define φ(y,x) = minj=1,···,p φj (y,x). ˜ Then φ(x,·) solves the visibility problem in the sense that ˜ φ(y,x) is positive if a viewer located in y is visible from x and negative otherwise. Lemma 3: ∇x φ˜ exists whenever all the ∇x φj exist. When this is the case, it has the expression: ! p p X Y ˜ ∇x φ(y,x) = ∇x φj (x,y) 1{φ (x.y) 0, P = {γ ∈ C 1 ([0,T ],Rd ) : ψ(γ(t)) > 0,t ∈ [0,T ]}
(2.4)
the set of all trajectories of D that do not cross the obstacles. The viewer starts from an initial location x0 ∈ D\Ω and moves along some path γ ∈ P. We then extend the definition of the visibility function as follows: φ(·,γ) = max φ(·,x) x∈γ
(2.5)
φ(y,γ) is positive if and only if there exists some point on γ from which y is visible and therefore the visible regions of a viewer with memory and moving along γ is exactly {φ(·,γ) > 0}. Then, for any path γ ∈ P we define the accumulated visibility level set function as: Φ : [0,T ] × D → R
(2.6)
. (t,y) 7→ Φ(t,y) = max φ(y,γ(s)) s∈[0,t]
where H denotes the Heaviside function.
where H is a Lipschitz approximation of the Heaviside function and M > 0 is some large number. So r(x) is 1 if ψ(x) > 0 and M if ψ(x) ≤ 0. Then the solution of the eikonal equation k∇u(x)k = r(x), x ∈ W (2.11) u(x0 ) = 0 is such that u(x) represents the minimum distance from x0 to x in presence of obstacles. The shortest path joining the point x0 to any point x1 can be computed by solving the equation v(t) ˙ = −∇u(v(t)), 0 ≤ t < T (2.12) v(0) = x1 Where T > 0 is such that v(T ) = x0 . The geodesic joining x0 and x1 is exactly L(x0 ,x1 ) = {v(t) : 0 ≤ t ≤ T }. To maximize the cumulated visibility over all geodesics we solve the eikonal equation |∇u| = r where r = 1 + (M − 1)H(−ψ) and we call the solution u. We define a new function f on the domain by R H (φ(y,L(x0 ,x))dy, if ψ(x) > 0 D f (x) = (2.13) 0 otherwise where L(x0 ,x) is the geodesic from x0 to x and is computed using u; f (x) represent the cumulated area that would be obtained if the viewer followed the geodesic from x0 to x. Note that f is not necessarily continuous since for some x and some small δx the geodesic L(x0 ,x + δx) might be very different from L(x0 ,x) due to the presence of obstacles. Among all of the maximizers of f , we want to find the single one that minimizes u. To do so, define for any > 0 by: f = f 1{u≤}
(2.7)
Φ(t,·) encodes the knowledge that a viewer has of the environment at time t when he moves along the path γ. When D is compact, we can define the corresponding discovered area at time t as Z V (t) = H(Φ(t,y))dy (2.8) D
Lemma 4: Assume that ψ is K-Lipschitz on D and fix some y ∈ D. For any γ1 ,γ2 ∈ P |φ(y,γ2 ) − φ(y,γ1 )| ≤ Kkγ2 − γ1 k ∞ Lemma 5: For any path γ ∈ P, the corresponding accumulated visibility level set function Φ is solution of the following PDE on [0,T ] × D: ∂Φ + ˙ 1{φ(y,γ(t))=Φ(t,y)} , ∂t (t,y) = {h∇x φ(y,γ(t)), γ(t)i} Φ(0,y) = φ(y,γ(0)), y∈D (2.9) where we have denoted {z}+ = max{z,0}. The Eikonal equation, a special case of Hamilton-Jacobi for which efficient numerical schemes are available [10], [7], can be used to compute geodesic distances in a domain D ˚ containing obstacles. Define W = D\{x 0 } and . r(x) = 1 + (M − 1)H(−ψ(x)) (2.10)
(2.14)
So f (x) equals f (x) if x can be joined by a path shorter than and zero otherwise. Select n o 0 = min > 0 : max f (x) = max f (x) (2.15) x inD
x inD
Then choose x1 = argmax f0 (x) x∈D
(2.16)
x1 is the closest maximizer of f and is at a distance of 0 . So the geodesic L(x0 ,x1 ) provides the maximum cumulated visibility that can be obtained by following a geodesic starting from x0 and by construction it is the shortest that does so.
obstacle. Therefore, the point y is visible if ρx (πx (y)) ≤ |y − x| and invisible otherwise.
1 2
0.5
ρ
1.5
0
1
0.5
−0.5 0 0
−1 −1
Fig. 1. For the domain with three obstacles shown above and a starting location x0 = (−0.7,0.9), the values of f are shown on the left. The right figure shows the best geodesic and the corresponding visible regions. The path length is 2.8160 and the target point is x1 = (0.5,−1).
III. E XTENSION TO UNKNOWN ENVIRONMENTS Rather than following a “receding horizon” approach, where a path of fixed length is planned, and then followed only in part before re-planning, we follow a “bandit” approach, where the viewer can choose a point – at an arbitrary distance within the visible region – where the expected gain (once properly defined) is maximal. Because it is visible, the shortest path to that point is a straight line. The viewer now has the choice of being stubborn – travel until it reaches that point, regardless of evidence that becomes available in the process, and only then re-plan – or indecisive – aim towards that point, but re-plan as soon as new evidence becomes available. Of course, other combinations are possible, where re-planning is triggered by various mechanisms, or at regular intervals. Note that the evaluation of the expected gain has to be performed before an action is taken, because the actual gain is unknown until that point is reached. Therefore, it depends on the prior assumptions on the shape, size, number and distributions of obstacles in the occluded region. This is in line with the general framework of Value of Information, whereby the control is chosen to maximize the expected information gain, that in turn can be though of as an “energy” or “cost” to be optimized. In our case, the underlying distribution whose conditional entropy measures “information” is uniform and supported in the shadow area, since the only uncertainty is due to occlusion. The shape of objects can be arbitrary; we define a spherical coordinate system centered on x by y = x + rν where ν ∈ S d−1 and r = |y − x|. Then define the projection operator πx : D 7→ S d−1 , mapping a point onto the unit sphere centered at x, by πx (x + rν) = ν. The data returned by the sensor can be summarized by a function ρx : S d−1 7→ R defined as ρx (ν) = min{ min r,M } ¯ x+rν∈Ω
(3.17)
where M > 0 is some large number. ρx (ν) gives the distance to obstacles in the ν direction. ρx (ν) equals M if there is no
−0.5
0
0.5
1
2
3
θ
4
5
6
1
Fig. 2. Left: The domain contains three obstacles and the viewer is located at (−0.2,0.4). The figure on the right plots the corresponding sensed data ρ as a function of the direction θ. The blue dots on the left figure show the cartesian representation of ρ.
We then define a set G = {(θ,r) : r < ρx (θ)}
(3.18)
and construct a smooth distance function ϕ to the shadow boundary ∂Ω in polar coordinates using redistancing. The ϕ function therefore satisfies: ϕ(θ,r) > 0, if (θ,r) ∈ G ϕ(θ,r) < 0, if (θ,r) ∈ GC (3.19) ϕ(θ,r) = 0, if (θ,r) ∈ ∂G and can easily be converted from polar coordinates to cartesian coordinates. We define φx on D by: φx (y) = ϕ(θ,r)
(3.20)
where y = x + rν and ν in the vector of S d−1 in the θ direction. . From now on we use the notation φ(y,x) = φx (y). A. Derivation of the heuristic Assume that the viewer has traveled for some portion of a path γ, which has resulted in the accumulated visibility φ(·,γ) as in Figure 3. Each continuous portion of an obstacle terminates with the horizon points (occluding boundaries). The lines delimiting the visible region and tangent to the obstacle at the occluding points are called “horizon lines”. Note that they enclose invisible regions. Our heuristic is articulated in several steps. First, rather than determining the instantaneous control (velocity direction) that yields the maximum information (visibility) gain, we wish to determine the point from which, if we were teleported, we would have the maximum visibility gain. Thus the resulting algorithm will be intrinsically non-myopic and akin the socalled “bandit” problems studied in Artificial Intelligence. Then, among all target points, we restrict to those that are visible. That is because we cannot say a priori whether a point in the shadow area (A1 and A2 in Fig. 3) lies inside an obstacle (and therefore we cannot go there), or whether it
Monday, November 14, 2011
Fig. 3. Left: accumulated visibility for a small path γ starting at x0 in presence of two obstacles. The leftmost object ends with horizon points (occluding boundaries) e1 and e2 and generates horizon lines h1 and h2 . The area of the corresponding invisible region that they enclose is A1 . Right: x is a point within the visible region and y is a point lying on one of the horizons. θ(x,y) is the angle that the direction x − y makes with the perpendicular to the horizon. Monday, November 14, 2011
is viable. As a result, the shortest path to the target object is trivial to determine as it is a straight line. Of course, we cannot determine the information gain we would have by moving to a given point, because we do not know the shape of the obstacle in the invisible region. Therefore, we can at most compute the expected visibility gain, where the expectation is taken with respect to the distribution of the obstacle field. This would appear to be principled, but it would require an explicit choice of shape prior, that is problematic. Instead of choosing an arbitrary prior, we choose an arbitrary energy (value) that we will try to minimize. Later, we will validate this choice empirically, and bound the complexity of the exploration process relative to a class of shapes (polygons). Ideally, we would think of such a value as the expected gain in visibility. However, since there is no explicit distribution against which to compute the expectation, we refer to it as “energy” to avoid confusion. To derive the energy, we refer to a simple example depicted in Fig. 3: If the viewer were to jump to a location x, he would see the horizon lines h2 and h4 . Let y lie on one of these horizons and define θ(x,y) as the angle that x − y makes with the outward unit normal at y. In order to see what is beyond the horizon, the viewer should choose a location that makes θ(x,y) as close to zero as possible for most of the y’s lying on the horizons. When the angle is small, that also means that the viewer would have the most direct path to cross the horizon. Therefore, the following energy quantifies “how exposed” the horizons are, as well as how fast it would be to cross them: Z X E(x) = Ai f (θ(x,y))dy. (3.21) y∈hi i:hi visible from x Here, Ai is the area of the invisible region enclosed by hi and f is a positive strictly decreasing function of θ. Before we proceed, we need to define more precisely what visible horizon means. With the information available, it is clear that the horizon lines h2 and h3 will become completely visible. However, we do not know beforehand whether any part of h1 and h4 will be visible since we does not know what lies
inside the invisible regions (in white). It is possible (although unlikely) that the obstacle occupies the entire invisible region (gray and white), and therefore the visibility gain from moving to x would be zero. It is also possible that the obstacle is a “thin shell” and the majority of the invisible region can be uncovered. That, again, depends on the shape of the obstacles, which we do not know a-priori. We consider the invisible regions as obstacles and therefore assume that a point y lying on a horizon is invisible if the segment [x,y] crosses the invisible region. With this additional assumption, h1 and h3 are considered invisible from x in Figure 3. As we will see now, the level set framework provides powerful tools that will allow us to mathematically formalize the above. B. Energy formulation The zero level set of φ can be extracted by applying a Dirac operator, approximated via δ (x) = 2 χ[− 2 , 2 ] (x)cos2 ( πx ).
Fig. 4. Left: Visible regions for a domain with two circular obstacles. Right: The zero-level set of the corresponding level set visibility function.
As seen in Figure 4, δ (φ) emphasizes the horizon lines but also includes the visible parts of object surfaces that must instead be discarded. We do so by masking out the nontangent portion via M (y) = minx∈γ χ]0,+∞] (k∇x φ(y,x)k), so that M δ (φ) includes horizon lines only. For any x in the visible region, a point y on a horizon line is visible from x if and only if the segment [x,y] does not intersect with the invisible region. We therefore introduce a new level set function ξ on D × D by considering {φ ≤ 0} as obstacles ξ(y,x) = min φ(z). (3.22) z∈[x,y]
The larger the invisible region enclosed, the more “attractive” the horizon lines should be. To capture this, we define a function A : D 7→ R such that if φ(y) > 0 then A(y) = 0 and if φ(y) ≤ 0 then A(y) equals the relative area of the invisible region that y belongs to. We define the energy E(x) to be Z H(φ(x))
f (θ(x,y))A(y)M (y)H(ξ(y,x)δ (φ(y))k∇φ(y)kdy y∈D
(3.23)
∇φ(y) x−y where cos(θ(x,y)) = kx−yk · k∇φ(y)k . For f we use f (t) = 0.5 + 0.5cos(t) (Fig. 5).
C. Algorithm We propose two versions of the exploration algorithm. Both iteratively calculate and update both the visibility and the energy, and find the location that maximizes it at each step.
1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
−1 −1
1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
−1 −1
−0.5
−0.5
0
0
0.5
0.5
1
1
Fig. 5. (Top) The viewer initially located at x0 = (0,−04) has moved to x1 = (0.4,0.8) (left). When we recalculate the energy with these new visibility regions, we obtain the figure on the right. (Bottom) When the viewer is at initial location x0 = (0,−04) (left) the corresponding energy (right) is maximized near the point (0.4,0.8). The black dots on the left figure are the point where the energy is evaluated.
In Alg. 1, the viewer moves towards the location with the highest energy and recalculates the strategy when it reaches the target (“stubborn” explorer). In Alg. 2, the explorer re-plans at regular spatial intervals along the planned path. This allows changing direction along the way. In both cases, exploration terminates when the energy is zero. Algorithm 1 (stubborn) Require: x0 1: k ← 0 2: Initialize R φ ← φ(·,x0 ) 3: val ← D H(φ) 4: while true do 5: E ←0 6: Compute E on {φk > 0}. 7: if maxE = 0 then 8: break 9: end if 10: Select xnext = argmaxE(x) 11: Move from xk to xnext by following L(xk ,xnext ) and do φ ← max{φ,maxz∈L(xk ,xnext ) φ(·,z)} while moving. R 12: val ← D H(φ) 13: xk+1 ← xnext 14: k ←k+1 15: end while IV. E XPERIMENTAL ASSESSMENT To evaluate the difference between stubborn and nonadaptive replanning, we performed randomized trials and
Algorithm 2 (non-adaptive replanning) Require: x0 Require: h > 0 1: k ← 0 2: Initialize R φ ← φ(·,x0 ) 3: val ← D H(φ) 4: while true do 5: E ←0 6: Compute E on {φk > 0}. 7: if maxE = 0 then 8: break 9: end if 10: Select xnext = argmaxE(x) and compute L(xk ,xnext ) 11: Travel a distance h on L(xk ,xnext ), call xstop the termination point and do φ ← max{φ,maxz∈L(xk ,xstop ) φ(·,z)} while moving. R 12: val ← D H(φ) 13: xk+1 ← xstop 14: k ←k+1 15: end while
recorded the length of the path before termination of the algorithm. Although we do not compute the absolute minimum length, which would require knowledge of the map, and the solution of the optimal control problem mentioned in the introduction, the difference between the minimum and the actual length relates to regret, a common measure of performance for exploration algorithms. Therefore, total length relates to regret. We also recorded the computational complexity, that is monotonically related to the number of re-plannings before termination. Unsurprisingly, the stubborn strategy requires fewer re-plannings. Perhaps surprisingly, the algorithms exhibit very similar regret distributions, so the stubborn strategy does not produce appreciably longer paths. Of course, this is true in the average. One can easily construct examples of particular scenes and initial conditions where the stubborn algorithm will travel a significantly longer total path. We performed the experiments on an environment containing three random obstacles, with different shapes. The stubborn algorithm takes on average 5 steps to complete the exploration. With re-planning, the algorithm takes on average 12 re-planning steps, for a total distance traveled that is marginally smaller. The next experiment is meant to validate the heuristic in terms of total distance traveled (regret) and number of steps (computational cost) on a random sample of scenes. The samples are independently generated by an arbitrary placement of 10 to 12 non-intersecting objects with varying shape and size. The viewer’s starting location is also randomized in the domain not occupied by the obstacles. Algorithm 2 re-plans whenever the distance traveled since the last decision reaches 0.5 units. The statistics are collected over 1000 runs. Figure 6 shows the results of the experiment. The left column corresponds to Algorithm 1 and the right column to
Algorithm 2. The number of steps are shown in the histograms and the total distances traveled are shown in the scatterplots (bottom row). Number of steps in the algorithm
Number of steps in the algorithm
500
120 100
400
80 300 60 200 40 100 0 0
20 10
20
30
40
0 0
50
10
Total distance traveled 250
200
200
150
150
100
100
50
50
0 0
10
20
30
0 0
40
Relative area versus distance traveled
30
40
50
10
20
30
40
Relative area versus distance traveled
1 Relative area
1 Relative area
20
Total distance traveled
250
0.8 0.6 0.4 0.2 0 0
there exists some η > 0 such that (∀)i ∈ {1,··· ,M },(∃)zi ∈ D : B(zi ,η) ⊂ Ωi . Finally, we assume that there exists some . constant δ2 > δ1 such that dist(Ωi ,Ωj ) = minx∈Ωi ,y∈Ωj |x − y| ≥ δ2 for any i 6= j. Our main result is the following: Theorem 5.1: Let 0 < < δ1 . The exploration algorithm alg() takes O(M Q) + O( 15 ) steps to terminate. At termination, the uncovered area is bounded by O(2 ). The proof is rather lengthy and omitted due to space limitation. It will be included in a forthcoming journal version of this manuscript.
0.8 0.6 0.4 0.2
2
4 6 8 Distance traveled
10
VI. D ISCUSSION We have presented an exploratory path planning algorithm with provable performance bounds, based on a heuristic measure of the “value” of future measurements. We have validated it with repeated trials where the shape, size, location and number of objects was randomized, and evaluated the complexity cost (number of re-plannings) and regret (length of path to termination). Our approach has several limitations: we assuming noiseless range measurements, so the only uncertainty is due to visibility. We assumed knowledge of the occluding boundaries. Our algorithm assumes a bounded domain D. Extending the approach to an unbounded unknown domain is an important generalization needed to make the method practical.
0 0
2
4
6 8 10 Distance traveled
12
Fig. 6. From top-left to bottom-right: Number of steps for Algs. 1 and 2, Total distance traveled for Algs. 1 and 2, relative gain in visibility against distance traveled for Alg. 1 and 2.
Acknowledgement. We thank Yana Landa and Ryo Takei for discussions during the early stages of this project. Research was supported by DARPA MSEE FA8650-11-1-7156, and ARO MURI W911NF-11-1-0391. R EFERENCES
As we can see, most of the paths resulting from Algorithm 1 are made of 8 steps or less, which is less than the number of obstacles. The corresponding path lengths range between 0 and 15 with an average of 6.63. In comparison, Algorithm 2 requires 8 to 28 steps to converge which is due to the fact that the strategy is recalculated more often that in the first case. The total path lengths though still range between 0 and 15, with an average of 5.86 This is slightly better than Algorithm 1, but not as dramatic as one would expect considering the naive approach of the stubborn explorer. Figure 6 shows the relative area of the discovered regions against the traveled distance. The next section describes some analysis aimed at bounding the performance of the algorithm described. V. A NALYSIS In this section we derive a bound on the number of replanning steps as a function of the complexity of the environment, assumed to be bounded with diam(D) ≤ K for some K > 0. The objects (Ωi )i=1,···,M are assumed polygonal for ease of proofs, with finite complexity Q. That is, for each i ∈ {1,··· ,M }, Ωi is described by Ni ≤ Q vertices (vji )j=1,···,Ni . We assume there exists δ1 > 0 such that mini=1,···,M minp6=q |vpi − vqi | ≥ δ1 . In other words, the length of the segments that separate two vertices is at least . Also,
[1] A. Ayvaci, M. Raptis, and S. Soatto. Sparse occlusion detection with optical flow. Intl. J. of Comp. Vision, (in press) 2011. [2] O. Enchev and D. W. Stroock. Rademacher’s theorem for wiener functionals. The Annals of Probability, 21(1):25–33, 1993. [3] Y.-H. R. Tsai, L.-T. Cheng, S. J. Osher, and H. K. Zhao. Fast sweeping algorithms for a class of hamilton-jacobi equations. SIAM Journal on Numerical Analysis, 41:673–694, 2003. [4] C. Y. Kao and R. Tsai. Properties of a level set algorithm for the visibility problems. Journal of Scientific Computing, 35:170–191, 2008. 10.1007/s10915-008-9197-5. [5] Y. Landa and R. Tsai. Visibility of point clouds and exploratory path planning in unknown environments. CAM Report 08-28, 2008. [6] S. J. Osher and J. A. Sethian. Fronts propagating with curvature dependent speed: Algorithms based on hamilton-jacobi formulations. JOURNAL OF COMPUTATIONAL PHYSICS, 79(1):12–49, 1988. [7] J. A. Sethian and A. Vladimirsky. Ordered upwind methods for static hamilton-jacobi equations. Proc. Natl. Acad. Sci. USA, 98(20):11069?11074 (electronic), 2001. [8] B. Tovar, R. Murrieta-cid, and S. M. Lavalle. Distance-optimal navigation in an unknown environment without sensing distances. IEEE Transactions on Robotics, 23, 2007. [9] Y.H. R. Tsai, L. T. Cheng, S. Osher, P. Burchard, and G. Sapiro. Visibility and its dynamics in a pde based implicit framework. J. COMPUTATIONAL PHYSICS, 199:260–290, 2004. [10] J. N. Tsitsiklis. Efficient algorithms for globally optimal trajectories. IEEE TRANSACTIONS ON AUTOMATIC CONTROL, 40(9):1528–1538, 1995. [11] R. Tsai Y. Landa and L.-T. Cheng. Visibility of point clouds and mapping of unknown environments. Springer Lecture Notes un Computational Science and Engineering, pages 1014–1025, 2006.
Richard Y.-H. Tsai†
Stefano Soatto§
§ University of California, Los Angeles, http://vision.ucla.edu † University of Texas at Austin, [email protected]
Abstract—We tackle the problem of exploring an unknown environment using a range sensor, where most of the uncertainty is due to the presence of opaque objects of unknown and arbitrary geometry, topology and configuration. Information-gathering control aims to minimize the residual uncertainty, resulting in an optimal control problem that is essentially intractable. We propose a surrogate measure of uncertainty that is efficient to compute and results in guaranteed performance bounds. The resulting algorithm is proven to complete exploration within a prescribed accuracy in a finite number of steps, depending in the complexity of the environment. Index Terms—Path planning, unknown environment, exploration, motion control, visibility computation, 3d reconstruction, mapping, localization, slam.
I. I NTRODUCTION We consider a spatial domain represented by a subset of the real plane. Obstacles, or “objects,” are simply-connected subsets of the domain. An omni-directional range sensor (or “viewer”) is represented by a point on the domain, that can move continuously in the region not occupied by obstacles, and yields a measurement of the distance from the sensor to each visible obstacle. Given an unknown environment (domain and obstacle field) and a starting point x0 , we are interested in designing a path of finite length at the end of which the viewer will have “seen” all obstacles. This can be formalized as an optimal control problem with the resulting Hamilton-Jacobi-Bellmann partial differential equation (PDE), or (discrete) sequential decision process. In either case, the resulting optimization is intractable for all but the most trivial cases. Our goal is therefore to devise surrogates or heuristics that result in algorithms that are provably sufficient, in the sense of eventually discovering all the visible region of the domain, and possibly efficient, in the sense of doing so in a finite number of steps depending on the complexity or volume of the environment. Greedy exploratory strategies aim to minimize, at each step, the residual uncertainty about the environment, that is the conditional entropy of the “map” given the measurements. In our case, where the only uncertain is due to visibility, we can think of the map as a probability distribution that is uniform and equal to zero in the part of the domain that has been seen, and positive otherwise. The maximally informative strategy then corresponds to maximizing the discovered area. Therefore, efficient computation of visibility is an important component of an exploratory path planner. For a known environment, this problem has been studied extensively in computational geometry.
When obstacles are represented implicitly as the zero level set of a function, [6] one can determine visibility by comparing the geodesic and Euclidean distances between the observer x0 and a point x solving the Eikonal equation. If ψ is the signed distance function to the objects surfaces, fast sweeping methods [3] can be used: Following the notation of [9], let D be some bounded domain; for any x0 ∈ D, the visibility function from x0 is generated via φ(x) =
min
ψ(z), x ∈ D
z∈L(x0 ,x)
(1.1)
where L(x0 ,x) is the segment [x0 ,x], and φ can be computed by solving the PDE y−x y−x = min{H(φ − ψ)∇ψ · |y−x| ,0} ∇φ · |y−x| (1.2) φ(x0 ) = ψ(x0 ) if the environment is known [4]. When the obstacles are unknown, [8], [5], [11] proposed algorithms to construct piecewise linear paths so that any point on the surface of the obstacles is seen by at least one vertex of the path. Each continuous portion of the visible boundary terminates with the occluding boundaries, or “horizon points” or edges on the visibility map. When a new horizon appears, it is stored in a list, and removed once explored. The algorithm terminates when the list L is empty. It should be noted that occluding boundaries are detectable both with range sensors as well as with optical sensors under suitable conditions [1]. We introduce an exploration algorithm that is guaranteed to be sufficient, in the sense of discovering almost all the visible area in a finite number of steps, and efficient, in the sense of requiring a small number of re-planning steps that is bounded depending on the complexity of the environment. We prove the results for the case where objects are polygonal, although the algorithm does not make that assumption, and can handle obstacles of arbitrary shape. II. BACKGROUND AND NOTATION Obstacles, or “objects, are denoted by Ω ⊂ D and their surfaces (boundaries) by Γ = ∂Ω. The obstacles are perfectly light-absorbing and cannot be crossed by the observer. Note that Ω does not uniquely define ψ. A common choice is the signed distance to Γ, which can be computed efficiently [3]. Lemma 1: Assume that ψ is Lipschitz continuous on D, that is, there exists some K > 0 such that |ψ(x2 ) − ψ(x1 )| ≤ K|x2 − x1 | for any x1 ,x2 ∈ D. Then, for any y ∈ D, |φ(y,x2 ) − φ(y,x1 )| ≤ K|x2 − x1 |
Corollary 2.1: Since φ(x,y) = φ(y,x) for any x,y,∈ D, under the same assumptions as lemma 1, φ(·,x) is also KLipschitz on D for any x ∈ D. |φ(y2 ,x) − φ(y1 ,x)| ≤ K|y2 − y1 | for any y1 ,y2 ∈ D Theorem 2.2: (Rademacher, 1919) Let n and m denote two positive integers and U an open subset of Rn . If f : U → Rm is locally Lipschitz, then it is differentiable at almost every x ∈ U (with respect to the Lebesgue measure). A proof of this result can be found in [2]. We can immediately state the following corollary: Corollary 2.3: Assume that ψ is K-Lipschitz for some K > 0. Then for any y ∈ D, ∇x φ(y,·) is defined almost everywhere on D. When multiple objects are present, ψ can be expressed using a number p of level set functions (ψi )i=1,···,p . Lemma 2: Assume that the scene can be described with ψ(x) = minj=1,···,p ψj (x). For any j ∈ {1,··· ,p}, define φj (y,x) = mint∈[0,1] ψj (x + t(y,x)). ˜ Finally, define φ(y,x) = minj=1,···,p φj (y,x). ˜ Then φ(x,·) solves the visibility problem in the sense that ˜ φ(y,x) is positive if a viewer located in y is visible from x and negative otherwise. Lemma 3: ∇x φ˜ exists whenever all the ∇x φj exist. When this is the case, it has the expression: ! p p X Y ˜ ∇x φ(y,x) = ∇x φj (x,y) 1{φ (x.y) 0, P = {γ ∈ C 1 ([0,T ],Rd ) : ψ(γ(t)) > 0,t ∈ [0,T ]}
(2.4)
the set of all trajectories of D that do not cross the obstacles. The viewer starts from an initial location x0 ∈ D\Ω and moves along some path γ ∈ P. We then extend the definition of the visibility function as follows: φ(·,γ) = max φ(·,x) x∈γ
(2.5)
φ(y,γ) is positive if and only if there exists some point on γ from which y is visible and therefore the visible regions of a viewer with memory and moving along γ is exactly {φ(·,γ) > 0}. Then, for any path γ ∈ P we define the accumulated visibility level set function as: Φ : [0,T ] × D → R
(2.6)
. (t,y) 7→ Φ(t,y) = max φ(y,γ(s)) s∈[0,t]
where H denotes the Heaviside function.
where H is a Lipschitz approximation of the Heaviside function and M > 0 is some large number. So r(x) is 1 if ψ(x) > 0 and M if ψ(x) ≤ 0. Then the solution of the eikonal equation k∇u(x)k = r(x), x ∈ W (2.11) u(x0 ) = 0 is such that u(x) represents the minimum distance from x0 to x in presence of obstacles. The shortest path joining the point x0 to any point x1 can be computed by solving the equation v(t) ˙ = −∇u(v(t)), 0 ≤ t < T (2.12) v(0) = x1 Where T > 0 is such that v(T ) = x0 . The geodesic joining x0 and x1 is exactly L(x0 ,x1 ) = {v(t) : 0 ≤ t ≤ T }. To maximize the cumulated visibility over all geodesics we solve the eikonal equation |∇u| = r where r = 1 + (M − 1)H(−ψ) and we call the solution u. We define a new function f on the domain by R H (φ(y,L(x0 ,x))dy, if ψ(x) > 0 D f (x) = (2.13) 0 otherwise where L(x0 ,x) is the geodesic from x0 to x and is computed using u; f (x) represent the cumulated area that would be obtained if the viewer followed the geodesic from x0 to x. Note that f is not necessarily continuous since for some x and some small δx the geodesic L(x0 ,x + δx) might be very different from L(x0 ,x) due to the presence of obstacles. Among all of the maximizers of f , we want to find the single one that minimizes u. To do so, define for any > 0 by: f = f 1{u≤}
(2.7)
Φ(t,·) encodes the knowledge that a viewer has of the environment at time t when he moves along the path γ. When D is compact, we can define the corresponding discovered area at time t as Z V (t) = H(Φ(t,y))dy (2.8) D
Lemma 4: Assume that ψ is K-Lipschitz on D and fix some y ∈ D. For any γ1 ,γ2 ∈ P |φ(y,γ2 ) − φ(y,γ1 )| ≤ Kkγ2 − γ1 k ∞ Lemma 5: For any path γ ∈ P, the corresponding accumulated visibility level set function Φ is solution of the following PDE on [0,T ] × D: ∂Φ + ˙ 1{φ(y,γ(t))=Φ(t,y)} , ∂t (t,y) = {h∇x φ(y,γ(t)), γ(t)i} Φ(0,y) = φ(y,γ(0)), y∈D (2.9) where we have denoted {z}+ = max{z,0}. The Eikonal equation, a special case of Hamilton-Jacobi for which efficient numerical schemes are available [10], [7], can be used to compute geodesic distances in a domain D ˚ containing obstacles. Define W = D\{x 0 } and . r(x) = 1 + (M − 1)H(−ψ(x)) (2.10)
(2.14)
So f (x) equals f (x) if x can be joined by a path shorter than and zero otherwise. Select n o 0 = min > 0 : max f (x) = max f (x) (2.15) x inD
x inD
Then choose x1 = argmax f0 (x) x∈D
(2.16)
x1 is the closest maximizer of f and is at a distance of 0 . So the geodesic L(x0 ,x1 ) provides the maximum cumulated visibility that can be obtained by following a geodesic starting from x0 and by construction it is the shortest that does so.
obstacle. Therefore, the point y is visible if ρx (πx (y)) ≤ |y − x| and invisible otherwise.
1 2
0.5
ρ
1.5
0
1
0.5
−0.5 0 0
−1 −1
Fig. 1. For the domain with three obstacles shown above and a starting location x0 = (−0.7,0.9), the values of f are shown on the left. The right figure shows the best geodesic and the corresponding visible regions. The path length is 2.8160 and the target point is x1 = (0.5,−1).
III. E XTENSION TO UNKNOWN ENVIRONMENTS Rather than following a “receding horizon” approach, where a path of fixed length is planned, and then followed only in part before re-planning, we follow a “bandit” approach, where the viewer can choose a point – at an arbitrary distance within the visible region – where the expected gain (once properly defined) is maximal. Because it is visible, the shortest path to that point is a straight line. The viewer now has the choice of being stubborn – travel until it reaches that point, regardless of evidence that becomes available in the process, and only then re-plan – or indecisive – aim towards that point, but re-plan as soon as new evidence becomes available. Of course, other combinations are possible, where re-planning is triggered by various mechanisms, or at regular intervals. Note that the evaluation of the expected gain has to be performed before an action is taken, because the actual gain is unknown until that point is reached. Therefore, it depends on the prior assumptions on the shape, size, number and distributions of obstacles in the occluded region. This is in line with the general framework of Value of Information, whereby the control is chosen to maximize the expected information gain, that in turn can be though of as an “energy” or “cost” to be optimized. In our case, the underlying distribution whose conditional entropy measures “information” is uniform and supported in the shadow area, since the only uncertainty is due to occlusion. The shape of objects can be arbitrary; we define a spherical coordinate system centered on x by y = x + rν where ν ∈ S d−1 and r = |y − x|. Then define the projection operator πx : D 7→ S d−1 , mapping a point onto the unit sphere centered at x, by πx (x + rν) = ν. The data returned by the sensor can be summarized by a function ρx : S d−1 7→ R defined as ρx (ν) = min{ min r,M } ¯ x+rν∈Ω
(3.17)
where M > 0 is some large number. ρx (ν) gives the distance to obstacles in the ν direction. ρx (ν) equals M if there is no
−0.5
0
0.5
1
2
3
θ
4
5
6
1
Fig. 2. Left: The domain contains three obstacles and the viewer is located at (−0.2,0.4). The figure on the right plots the corresponding sensed data ρ as a function of the direction θ. The blue dots on the left figure show the cartesian representation of ρ.
We then define a set G = {(θ,r) : r < ρx (θ)}
(3.18)
and construct a smooth distance function ϕ to the shadow boundary ∂Ω in polar coordinates using redistancing. The ϕ function therefore satisfies: ϕ(θ,r) > 0, if (θ,r) ∈ G ϕ(θ,r) < 0, if (θ,r) ∈ GC (3.19) ϕ(θ,r) = 0, if (θ,r) ∈ ∂G and can easily be converted from polar coordinates to cartesian coordinates. We define φx on D by: φx (y) = ϕ(θ,r)
(3.20)
where y = x + rν and ν in the vector of S d−1 in the θ direction. . From now on we use the notation φ(y,x) = φx (y). A. Derivation of the heuristic Assume that the viewer has traveled for some portion of a path γ, which has resulted in the accumulated visibility φ(·,γ) as in Figure 3. Each continuous portion of an obstacle terminates with the horizon points (occluding boundaries). The lines delimiting the visible region and tangent to the obstacle at the occluding points are called “horizon lines”. Note that they enclose invisible regions. Our heuristic is articulated in several steps. First, rather than determining the instantaneous control (velocity direction) that yields the maximum information (visibility) gain, we wish to determine the point from which, if we were teleported, we would have the maximum visibility gain. Thus the resulting algorithm will be intrinsically non-myopic and akin the socalled “bandit” problems studied in Artificial Intelligence. Then, among all target points, we restrict to those that are visible. That is because we cannot say a priori whether a point in the shadow area (A1 and A2 in Fig. 3) lies inside an obstacle (and therefore we cannot go there), or whether it
Monday, November 14, 2011
Fig. 3. Left: accumulated visibility for a small path γ starting at x0 in presence of two obstacles. The leftmost object ends with horizon points (occluding boundaries) e1 and e2 and generates horizon lines h1 and h2 . The area of the corresponding invisible region that they enclose is A1 . Right: x is a point within the visible region and y is a point lying on one of the horizons. θ(x,y) is the angle that the direction x − y makes with the perpendicular to the horizon. Monday, November 14, 2011
is viable. As a result, the shortest path to the target object is trivial to determine as it is a straight line. Of course, we cannot determine the information gain we would have by moving to a given point, because we do not know the shape of the obstacle in the invisible region. Therefore, we can at most compute the expected visibility gain, where the expectation is taken with respect to the distribution of the obstacle field. This would appear to be principled, but it would require an explicit choice of shape prior, that is problematic. Instead of choosing an arbitrary prior, we choose an arbitrary energy (value) that we will try to minimize. Later, we will validate this choice empirically, and bound the complexity of the exploration process relative to a class of shapes (polygons). Ideally, we would think of such a value as the expected gain in visibility. However, since there is no explicit distribution against which to compute the expectation, we refer to it as “energy” to avoid confusion. To derive the energy, we refer to a simple example depicted in Fig. 3: If the viewer were to jump to a location x, he would see the horizon lines h2 and h4 . Let y lie on one of these horizons and define θ(x,y) as the angle that x − y makes with the outward unit normal at y. In order to see what is beyond the horizon, the viewer should choose a location that makes θ(x,y) as close to zero as possible for most of the y’s lying on the horizons. When the angle is small, that also means that the viewer would have the most direct path to cross the horizon. Therefore, the following energy quantifies “how exposed” the horizons are, as well as how fast it would be to cross them: Z X E(x) = Ai f (θ(x,y))dy. (3.21) y∈hi i:hi visible from x Here, Ai is the area of the invisible region enclosed by hi and f is a positive strictly decreasing function of θ. Before we proceed, we need to define more precisely what visible horizon means. With the information available, it is clear that the horizon lines h2 and h3 will become completely visible. However, we do not know beforehand whether any part of h1 and h4 will be visible since we does not know what lies
inside the invisible regions (in white). It is possible (although unlikely) that the obstacle occupies the entire invisible region (gray and white), and therefore the visibility gain from moving to x would be zero. It is also possible that the obstacle is a “thin shell” and the majority of the invisible region can be uncovered. That, again, depends on the shape of the obstacles, which we do not know a-priori. We consider the invisible regions as obstacles and therefore assume that a point y lying on a horizon is invisible if the segment [x,y] crosses the invisible region. With this additional assumption, h1 and h3 are considered invisible from x in Figure 3. As we will see now, the level set framework provides powerful tools that will allow us to mathematically formalize the above. B. Energy formulation The zero level set of φ can be extracted by applying a Dirac operator, approximated via δ (x) = 2 χ[− 2 , 2 ] (x)cos2 ( πx ).
Fig. 4. Left: Visible regions for a domain with two circular obstacles. Right: The zero-level set of the corresponding level set visibility function.
As seen in Figure 4, δ (φ) emphasizes the horizon lines but also includes the visible parts of object surfaces that must instead be discarded. We do so by masking out the nontangent portion via M (y) = minx∈γ χ]0,+∞] (k∇x φ(y,x)k), so that M δ (φ) includes horizon lines only. For any x in the visible region, a point y on a horizon line is visible from x if and only if the segment [x,y] does not intersect with the invisible region. We therefore introduce a new level set function ξ on D × D by considering {φ ≤ 0} as obstacles ξ(y,x) = min φ(z). (3.22) z∈[x,y]
The larger the invisible region enclosed, the more “attractive” the horizon lines should be. To capture this, we define a function A : D 7→ R such that if φ(y) > 0 then A(y) = 0 and if φ(y) ≤ 0 then A(y) equals the relative area of the invisible region that y belongs to. We define the energy E(x) to be Z H(φ(x))
f (θ(x,y))A(y)M (y)H(ξ(y,x)δ (φ(y))k∇φ(y)kdy y∈D
(3.23)
∇φ(y) x−y where cos(θ(x,y)) = kx−yk · k∇φ(y)k . For f we use f (t) = 0.5 + 0.5cos(t) (Fig. 5).
C. Algorithm We propose two versions of the exploration algorithm. Both iteratively calculate and update both the visibility and the energy, and find the location that maximizes it at each step.
1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
−1 −1
1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
−1 −1
−0.5
−0.5
0
0
0.5
0.5
1
1
Fig. 5. (Top) The viewer initially located at x0 = (0,−04) has moved to x1 = (0.4,0.8) (left). When we recalculate the energy with these new visibility regions, we obtain the figure on the right. (Bottom) When the viewer is at initial location x0 = (0,−04) (left) the corresponding energy (right) is maximized near the point (0.4,0.8). The black dots on the left figure are the point where the energy is evaluated.
In Alg. 1, the viewer moves towards the location with the highest energy and recalculates the strategy when it reaches the target (“stubborn” explorer). In Alg. 2, the explorer re-plans at regular spatial intervals along the planned path. This allows changing direction along the way. In both cases, exploration terminates when the energy is zero. Algorithm 1 (stubborn) Require: x0 1: k ← 0 2: Initialize R φ ← φ(·,x0 ) 3: val ← D H(φ) 4: while true do 5: E ←0 6: Compute E on {φk > 0}. 7: if maxE = 0 then 8: break 9: end if 10: Select xnext = argmaxE(x) 11: Move from xk to xnext by following L(xk ,xnext ) and do φ ← max{φ,maxz∈L(xk ,xnext ) φ(·,z)} while moving. R 12: val ← D H(φ) 13: xk+1 ← xnext 14: k ←k+1 15: end while IV. E XPERIMENTAL ASSESSMENT To evaluate the difference between stubborn and nonadaptive replanning, we performed randomized trials and
Algorithm 2 (non-adaptive replanning) Require: x0 Require: h > 0 1: k ← 0 2: Initialize R φ ← φ(·,x0 ) 3: val ← D H(φ) 4: while true do 5: E ←0 6: Compute E on {φk > 0}. 7: if maxE = 0 then 8: break 9: end if 10: Select xnext = argmaxE(x) and compute L(xk ,xnext ) 11: Travel a distance h on L(xk ,xnext ), call xstop the termination point and do φ ← max{φ,maxz∈L(xk ,xstop ) φ(·,z)} while moving. R 12: val ← D H(φ) 13: xk+1 ← xstop 14: k ←k+1 15: end while
recorded the length of the path before termination of the algorithm. Although we do not compute the absolute minimum length, which would require knowledge of the map, and the solution of the optimal control problem mentioned in the introduction, the difference between the minimum and the actual length relates to regret, a common measure of performance for exploration algorithms. Therefore, total length relates to regret. We also recorded the computational complexity, that is monotonically related to the number of re-plannings before termination. Unsurprisingly, the stubborn strategy requires fewer re-plannings. Perhaps surprisingly, the algorithms exhibit very similar regret distributions, so the stubborn strategy does not produce appreciably longer paths. Of course, this is true in the average. One can easily construct examples of particular scenes and initial conditions where the stubborn algorithm will travel a significantly longer total path. We performed the experiments on an environment containing three random obstacles, with different shapes. The stubborn algorithm takes on average 5 steps to complete the exploration. With re-planning, the algorithm takes on average 12 re-planning steps, for a total distance traveled that is marginally smaller. The next experiment is meant to validate the heuristic in terms of total distance traveled (regret) and number of steps (computational cost) on a random sample of scenes. The samples are independently generated by an arbitrary placement of 10 to 12 non-intersecting objects with varying shape and size. The viewer’s starting location is also randomized in the domain not occupied by the obstacles. Algorithm 2 re-plans whenever the distance traveled since the last decision reaches 0.5 units. The statistics are collected over 1000 runs. Figure 6 shows the results of the experiment. The left column corresponds to Algorithm 1 and the right column to
Algorithm 2. The number of steps are shown in the histograms and the total distances traveled are shown in the scatterplots (bottom row). Number of steps in the algorithm
Number of steps in the algorithm
500
120 100
400
80 300 60 200 40 100 0 0
20 10
20
30
40
0 0
50
10
Total distance traveled 250
200
200
150
150
100
100
50
50
0 0
10
20
30
0 0
40
Relative area versus distance traveled
30
40
50
10
20
30
40
Relative area versus distance traveled
1 Relative area
1 Relative area
20
Total distance traveled
250
0.8 0.6 0.4 0.2 0 0
there exists some η > 0 such that (∀)i ∈ {1,··· ,M },(∃)zi ∈ D : B(zi ,η) ⊂ Ωi . Finally, we assume that there exists some . constant δ2 > δ1 such that dist(Ωi ,Ωj ) = minx∈Ωi ,y∈Ωj |x − y| ≥ δ2 for any i 6= j. Our main result is the following: Theorem 5.1: Let 0 < < δ1 . The exploration algorithm alg() takes O(M Q) + O( 15 ) steps to terminate. At termination, the uncovered area is bounded by O(2 ). The proof is rather lengthy and omitted due to space limitation. It will be included in a forthcoming journal version of this manuscript.
0.8 0.6 0.4 0.2
2
4 6 8 Distance traveled
10
VI. D ISCUSSION We have presented an exploratory path planning algorithm with provable performance bounds, based on a heuristic measure of the “value” of future measurements. We have validated it with repeated trials where the shape, size, location and number of objects was randomized, and evaluated the complexity cost (number of re-plannings) and regret (length of path to termination). Our approach has several limitations: we assuming noiseless range measurements, so the only uncertainty is due to visibility. We assumed knowledge of the occluding boundaries. Our algorithm assumes a bounded domain D. Extending the approach to an unbounded unknown domain is an important generalization needed to make the method practical.
0 0
2
4
6 8 10 Distance traveled
12
Fig. 6. From top-left to bottom-right: Number of steps for Algs. 1 and 2, Total distance traveled for Algs. 1 and 2, relative gain in visibility against distance traveled for Alg. 1 and 2.
Acknowledgement. We thank Yana Landa and Ryo Takei for discussions during the early stages of this project. Research was supported by DARPA MSEE FA8650-11-1-7156, and ARO MURI W911NF-11-1-0391. R EFERENCES
As we can see, most of the paths resulting from Algorithm 1 are made of 8 steps or less, which is less than the number of obstacles. The corresponding path lengths range between 0 and 15 with an average of 6.63. In comparison, Algorithm 2 requires 8 to 28 steps to converge which is due to the fact that the strategy is recalculated more often that in the first case. The total path lengths though still range between 0 and 15, with an average of 5.86 This is slightly better than Algorithm 1, but not as dramatic as one would expect considering the naive approach of the stubborn explorer. Figure 6 shows the relative area of the discovered regions against the traveled distance. The next section describes some analysis aimed at bounding the performance of the algorithm described. V. A NALYSIS In this section we derive a bound on the number of replanning steps as a function of the complexity of the environment, assumed to be bounded with diam(D) ≤ K for some K > 0. The objects (Ωi )i=1,···,M are assumed polygonal for ease of proofs, with finite complexity Q. That is, for each i ∈ {1,··· ,M }, Ωi is described by Ni ≤ Q vertices (vji )j=1,···,Ni . We assume there exists δ1 > 0 such that mini=1,···,M minp6=q |vpi − vqi | ≥ δ1 . In other words, the length of the segments that separate two vertices is at least . Also,
[1] A. Ayvaci, M. Raptis, and S. Soatto. Sparse occlusion detection with optical flow. Intl. J. of Comp. Vision, (in press) 2011. [2] O. Enchev and D. W. Stroock. Rademacher’s theorem for wiener functionals. The Annals of Probability, 21(1):25–33, 1993. [3] Y.-H. R. Tsai, L.-T. Cheng, S. J. Osher, and H. K. Zhao. Fast sweeping algorithms for a class of hamilton-jacobi equations. SIAM Journal on Numerical Analysis, 41:673–694, 2003. [4] C. Y. Kao and R. Tsai. Properties of a level set algorithm for the visibility problems. Journal of Scientific Computing, 35:170–191, 2008. 10.1007/s10915-008-9197-5. [5] Y. Landa and R. Tsai. Visibility of point clouds and exploratory path planning in unknown environments. CAM Report 08-28, 2008. [6] S. J. Osher and J. A. Sethian. Fronts propagating with curvature dependent speed: Algorithms based on hamilton-jacobi formulations. JOURNAL OF COMPUTATIONAL PHYSICS, 79(1):12–49, 1988. [7] J. A. Sethian and A. Vladimirsky. Ordered upwind methods for static hamilton-jacobi equations. Proc. Natl. Acad. Sci. USA, 98(20):11069?11074 (electronic), 2001. [8] B. Tovar, R. Murrieta-cid, and S. M. Lavalle. Distance-optimal navigation in an unknown environment without sensing distances. IEEE Transactions on Robotics, 23, 2007. [9] Y.H. R. Tsai, L. T. Cheng, S. Osher, P. Burchard, and G. Sapiro. Visibility and its dynamics in a pde based implicit framework. J. COMPUTATIONAL PHYSICS, 199:260–290, 2004. [10] J. N. Tsitsiklis. Efficient algorithms for globally optimal trajectories. IEEE TRANSACTIONS ON AUTOMATIC CONTROL, 40(9):1528–1538, 1995. [11] R. Tsai Y. Landa and L.-T. Cheng. Visibility of point clouds and mapping of unknown environments. Springer Lecture Notes un Computational Science and Engineering, pages 1014–1025, 2006.
