Transcript

Comput Optim Appl (2010) 45: 521–541DOI 10.1007/s10589-008-9185-6

A penalized nonparametric method for nonlinearconstrained optimization based on noisy data

Ronaldo Dias · Nancy L. Garcia ·Adriano Z. Zambom

Received: 26 October 2007 / Revised: 28 April 2008 / Published online: 14 May 2008© Springer Science+Business Media, LLC 2008

Abstract The objective of this study is to find a smooth function joining two points A

and B with minimum length constrained to avoid fixed subsets. A penalized nonpara-metric method of finding the best path is proposed. The method is generalized to thesituation where stochastic measurement errors are present. In this case, the proposedestimator is consistent, in the sense that as the number of observations increases thestochastic trajectory converges to the deterministic one. Two applications are imme-diate, searching the optimal path for an autonomous vehicle while avoiding all fixedobstacles between two points and flight planning to avoid threat or turbulence zones.

Keywords Autonomous vehicle · B-splines · Consistent estimator · Confidenceellipses · Constrained optimization · Nonparametric method

1 Introduction

Finding the shortest path between two points in the presence of obstacles is an im-portant problem in robotics, geographical information systems and in-flight planningof routes. A classical application in robotics is the search for optimal trajectories foran autonomous vehicle which has to move from point A to point B traveling theminimum possible distance while avoiding all fixed obstacles between these points.

R. Dias (�) · N.L. Garcia · A.Z. ZambomDepartamento de Estatística, Universidade Estadual de Campinas (UNICAMP), Rua Sergio Buarquede Holanda, 651, Cidade Universitaria Zeferino Vaz, Caixa Postal 6065, 13.081-970, Campinas,Sao Paulo, Brazile-mail: [email protected]

N.L. Garciae-mail: [email protected]

A.Z. Zambome-mail: [email protected]

522 R. Dias et al.

A similar problem is to find a flight planning that avoids threat or turbulence zones.This avoidance must be achieved respecting some further constraints. For instance,a safe distance r must be kept at all times between the vehicle and the obstacles,while avoiding further areas like craters. Moreover, the trajectory has to follow asmooth curve because vehicles or planes cannot make abrupt turns. For more de-tails about autonomous vehicles see information about DARPA Grand Challenge(http://www.darpa.mil/grandchallenge).

A large body of literature has emerged over the past two decades on motionplanning for car-like mobile robots and Unmanned Aerial Vehicles (UAV). See thebooks [6, 13, 19] for introductory material, and [14, 15] for up-to-date results. Manydeterministic approaches have been proposed for obstacle avoidance problems. Theycan be classified into search-based methods, geometric approaches, control theoret-ical methods and artificial potential field methods. Some authors use an exhaustivesearch-based method that explores the configuration space by propagating step mo-tions for some controls, see for example [4]. The usual approach using dynamic pro-gramming divides the state space into cells of specified dimensions and places therestrictions in each cell along prescribed headings. The computational cost of thisapproach increases as the cell sizes decrease and the number of allowed headings in-creases. Moreover, the paths must be smoothed out to avoid abrupt heading changes.When the threat zones are circular, the simplest solution for the problem consists ofstraight line segments and circular arcs. The possible segments are easily enumer-ated by a search algorithm. Asseo [2] proposed an algorithm based on a geometricconstruction that uses linear segments tangent to the threat periphery and circular seg-ments along the threat periphery, to obtain the shortest route between the starting andthe destination points using the principle of optimality. This proposal cannot be gen-eralized to non-circular threat zones. Graph theory has been used lately to constructplanar coverage strategies for autonomous vehicle, see for instance [23].

Optimization problems posed by paths connecting two points have intrinsic math-ematical interest and appear in a number of applications besides robotics. We men-tion, in particular, the mountain-pass problem, of importance in non-linear analysisand computational chemistry, which has been the object of a vast literature. See [16]for a good introduction to the subject. This problem, however, looks only to criticalpoints of a path, rather than to the determination of the whole path.

We propose a penalized optimization procedure to obtain an approximate solutionto the problem. We consider functions belonging to a finite-dimensional approxi-mating space generated by B-splines basis and impose a penalization on solutionsthat do not comply with the constrains. Our approach presents several advantages:(i) It transforms an infinite-dimensional problem into a finite-dimensional one and,in practice, only few coefficients have to be computed; (ii) it naturally changes theconstrained optimization into an unconstrained one, and (iii) it can easily deal withpop-up threats without increasing the run-time. However, perhaps the main novelty ofour approach is that it can incorporate non-homogeneous error measurements in thelocation of the obstacles/threat zones, in which case it yields a consistent stochasticpath. That is, it can treat situations in which the avoidance set is not known exactlybut it is observed through a random mechanism that adds a random noise. In thesecases it produces a stochastic solution that converges to the deterministic one as thenumber of independent observations increases.

A penalized nonparametric method for nonlinear constrained 523

In this paper, we will analyze the problem both under the deterministic and sto-chastic scenarios. The former is studied in Sect. 2. There we assume a vehicle/planewith perfect sensors that can find the obstacles/threat zones without error. In this case,the path planning is obtained by solving a penalized optimization problem. In Sect. 3we consider noisy determination of obstacles and find a stochastic solution based onindependent readings. This solution converges, as the number of readings increase,to the former deterministic solution. Furthermore, in Sect. 5 we consider briefly thestepwise case where the region of interest is split into s pieces and the problem has tobe solved sequentially in each sub-region. This corresponds to an obstacle field thatis not entirely known and a vehicle that cannot see the whole space. It also applies tosituations with pop-up threat zones. This partial vision case is important not only perse but also because it allows to construct trajectories that are not graphs of functions.In this case, the trajectory is constructed piecewisely using the same algorithm. Sim-ilar ideas were used in practice during the 2005 DARPA Challenge by Caltech Team,see [7].

2 An optimization problem

In this work, we study the following nonlinear constrained optimization problem.Let A = (xa, ya) and B = (xb, yb) be two points in R

2, with xa < xb . Without loss ofgenerality, we will consider A = (0,0) and B = (b,0) (if not, a change of coordinatescan be used). We search for a path with minimum length that can be represented asthe graph of a smooth function f

Graph (f ) = {(x, y) : x ∈ [xa, xb] and y = f (x)}.To be precise on what we called a smooth trajectory, we consider only functions f

belonging to a subset of the Sobolev space

H22 :=

{f : [xa, xb] → R,

∫f 2 +

∫(f ′)2 +

∫(f ′′)2 < ∞, f (xa) = ya,

f (xb) = yb

}.

This Sobolev space is convenient for our purposes because it consists of smoothfunctions that can be well approximated by uniform B-splines, see [21]. Moreover,we want to constrain the search to paths that comply with the restriction of avoidingan open region � ⊂ R

2 which represents a region around certain obstacles or threatzones. We need open regions in order to have a unique solution for the optimizationproblem (see explanation after (2.5)).

Therefore, the goal is to find a smooth function f belonging to H22 satisfying:

1. The trajectory has to go through the points A = (0,0) and B = (b,0), i.e. f (0) = 0and f (b) = 0.

2. The trajectory has to avoid some set � around the obstacles (or the threat zones),that is, Graph (f ) ∩ � = ∅ for some fixed open set �.

524 R. Dias et al.

3. The function f minimizes the trajectory in the sense that the length of Graph (f )

defined by

Q(f ) :=∫ b

0

√(1 + f ′(t)2)dt

is minimum.

Notice that the constrained minimization problem can be viewed as a penalizedproblem where the penalty is 0 or ∞ according to Graph (f ) ∩ � = ∅ (the path doesnot intercept the obstacle region) or not. That is, we want a solution of

minf ∈H2

2

∫ b

0

√(1 + f ′(t))2dt + J ∗(f ) (2.1)

where

J ∗(f ) ={

0, if Graph (f ) ∩ � = ∅,

∞, if Graph (f ) ∩ � = ∅.(2.2)

In general, the solution for (2.1) is very difficult to find or even, in some cases,nonexistent depending on the set �, because we are restricting ourselves to functionsbelonging to H2

2.For the sake of simplicity in notation, from now on we will consider that we

have L points in R2 with coordinates ξi = (wi, vi), i = 1, . . . ,L. Denote by N =

{ξ1, ξ2, . . . , ξL} and

� =L⋃

i=1

B(ξi, r)

where B(ξ, r) = {z ∈ R2 : d(z, ξ) < r} and d is the Euclidean distance. It is easy to

see this set up can be generalized in a straightforward manner to more general � sets.To overcome the problems posed by (2.1), first we approximate the penalization

J ∗(f ), by the smooth functional

Jψ,α,n(f ) = ψ�(Zα + √

H(r − d(f,N)))

(2.3)

where d(f,N) = inf{d(z, ξ) : z ∈ Graph (f ), ξ ∈ N}, � is the cumulative standardGaussian distribution, Zα is its αth percentile and (ψ,α,H) are tuning parame-ters. This penalization is convenient since it follows that Jψ,α,H (f ) → J ∗(f ) whenψ → ∞, α → 0 and H → ∞. In Sect. 2.1 we explain the roles of the tuning para-meters.

Second, it is well-known that H22 is an infinite-dimensional space, but it can be

well approximated by a finite dimensional space which is spanned by K (fixed) basisfunctions, such as Fourier expansion, wavelets, B-splines, natural splines, see forexample [10, 12, 18, 21, 22, 24]. In this work, following [21] we will fix K and asequence t = (t1, . . . , tK−2) and consider f belonging to the space HK spanned byB-splines with interior knot sequence t. That is,

f (x) = fθ (x) =K∑

j=1

θjBj (x) (2.4)

A penalized nonparametric method for nonlinear constrained 525

Fig. 1 Basis functions with 6knots placed att = (0.0,0.2,0.4,0.6,0.8,1.0)

where Bj are the well-known cubic B-spline basis (order 4) and θ = (θ1, . . . , θK) isa vector of unknown coefficients.

Figure 1 presents 6 B-splines functions for equally spaced knots in [0,1]. It iswell known that B-splines are splines which have smallest possible support. In otherwords, B-splines are zero in a large set. Furthermore, a stable evaluation of B-splineswith the aid of a recurrence relation is possible. For details, see [8].

Therefore, we want to find fθ ∈ HK , or equivalently θ = (θ1, . . . , θK) ∈ RK which

minimizes

Qα,ψ,r,H (θ) =∫ b

0

(1 +

(K∑

j=1

θjB′j (t)

)2)1/2

dt

+ ψ�

(Zα + √

H

(r − d

(K∑

j=1

θjBj (·),N)))

, (2.5)

subject to fθ (0) = 0, fθ (b) = 0. At this point we can see the need to consider onlyopen balls around the obstacles (in general, � to be an open set). If not, the minimumin (2.5) is not attainable.

Notice that the penalized approach is very appealing since it allows us to changethe constrained nonlinear problem of finding a function into an unconstrained non-linear problem of finding a vector on R

K−2. This problem was solved by using thefunction fminunc from MATLAB which is based on the well-known BFGS Quasi-Newton method with a mixed quadratic and cubic line search procedure.

2.1 The tuning parameters

Notice that the functionals J ∗ and Jψ,α,H depend on the function f only through itsdistance to the obstacle field (d(f,N)), thus with an abuse of notation we introducethe real-valued functions defined for x ∈ [0,∞) as

J ∗(x) ={

0, if x ≥ r,

∞, if x < r(2.6)

526 R. Dias et al.

and

Jψ,α,n(x) = ψ�(Zα + √

H(r − x)). (2.7)

The function Jψ,α,H is a continuous analog of J ∗. The roles of the tuning parameters(ψ,α,H) are ψα = Jψ,α,H (r), ψ = maxx≥0 Jψ,α,H (x) and H controls the steepnessof Jψ,α,H at the point r . They should be chosen in such way that, when the trajectorytries to violate the constraint (e.g., the distance between the vehicle and the obstacleshave to be bigger than r at all times, the plane has to avoid threat zones) the penaliza-tion is so much bigger than the gain in the distance that this trajectory cannot occur.Figure 2 shows the effect of the tuning parameter H on the penalty Jψ,α,H (f ), fora better visualization we fixed ψα = 30. However, for computation and numericalexamples we will use α.ψ = 0.05.

Tables 1 and 2 present some simulation results for � sets given by Figs. 3 and 4.Notice that as ψ increases (α decreases, recall that ψ = 0.05/α) and/or H increaseswe get closer and closer to the best trajectory until it stabilizes. From these simulatedresults, we choose ψ = 106b, α = 0.05/ψ e H = 106b.

How many knots? In fact, in approximation theory, one of the most challengingproblem is how to select the dimension of the approximant space. A similar problemis encountered in the field of image processing where the level of resolution needs tobe determined appropriately. Figure 5 presents several � sets with different degrees

Fig. 2 Effect of the tuningparameter H on the penaltyfunction Jψ,α,H (f ), ψ = 100,α = 0.3, r = 0.5

Table 1 Length of the chosentrajectory for � presented inFig. 3

3α × 105 10−3 10−2 10−1 1

n × 10−3

0.5 15.1311 15.1311 15.1311 15.1311

1.5 15.1286 15.1288 15.1290 15.1311

15 15.1241 15.1247 15.1248 15.1248

150 15.1237 15.1238 15.1238 15.1241

1500 15.1220 15.1220 15.1220 15.1221

15000 15.1211 15.1211 15.1211 15.1211

A penalized nonparametric method for nonlinear constrained 527

Table 2 Length of the chosentrajectory for � presented inFig. 4

6α × 105 10−3 10−2 10−1 1

n × 10−3

0.5 30.0794 30.0794 30.0794 30.0794

3 30.0794 30.0794 30.0794 30.0794

30 30.0766 30.0776 30.0777 30.0778

300 30.0778 30.0778 30.0778 30.0778

3000 30.0757 30.0757 30.0758 30.0758

30000 30.0756 30.0756 30.0756 30.0757

Fig. 3 Example of a � set

Fig. 4 Example of a � set

of difficulty for finding a trajectory. These fields were construct to test the strengthof the algorithm since the best route is not “easy”. We could see that increasing thenumber of interior knots above 4 did not bring any improvement. With 3 or 4 interiorknots we obtain the “best” possible fθ . If necessary, the choice of the number of knotscan be very adaptive, see for example [1, 5, 9, 11, 17, 20] and easily implemented.

3 A stochastic problem. Complete vision

In the previous section, we assumed that the � set is deterministic, in the applicationsthis would mean the sensors of the vehicle/plane can see the whole field and detect

528 R. Dias et al.

Fig. 5 Estimated “best” trajectory using 4 internal knots

with certainty the placement of the obstacles/threat zones. This is not realistic, thereis always a measurement error involved. In this section, we will suppose that thesensors can see the whole field (partial vision will be considered in Sect. 5) but insteadof seeing N , they see η = N + ε where ε = (ε1, . . . , εL) is the measurement error.Specifically, we will assume that

� =L⋃

=1

B((w, v), r)

but the observations are given by (W,V) = (w, v) + (ε1, ε2), = 1, . . . ,L,with (ε1, ε2) ∼ N2((0,0),�), = 1, . . . ,L, independent random variables with

A penalized nonparametric method for nonlinear constrained 529

covariance structure given by � (that can depend on the point (w, v)) given by

� =(

σ 2,1 ρσ,1σ,2

ρσ,1σ,2 σ 2,2

). (3.1)

This scenario incorporates several practical situations, for example larger variancefor darker spots, increasing variance depending on the distance to the obstacle/threatzone, some threat zones being easier to spot, etc.

Moreover, we have for each point, n independent readings. Thus, our data is com-posed of n readings of the point process ηi = {(W1,i , V1,i ), . . . , (WL,i,VL,i)} for i =1, . . . , n. Denote W = (W,1, . . . ,W,n) and V = (V,1, . . . , V,n), = 1, . . . ,L.

For a sequence γn ∈ (0,1), the proposed estimator for fθ is the function

fγn

θ̂n

(x) =K∑

j=1

θ̂jBj (x) (3.2)

where θ̂n is the solution of the minimization problem

θ̂n = arg minQα,ψ,r,H,n(θ) (3.3)

subject to fγn

θ (0) = 0, fγn

θ (b) = 0, where

Qα,ψ,r,H,n(θ) =∫ b

0

(1 +

(K∑

j=1

θjB′j (t)

)2)1/2

dt

+ ψ�

(Zα + √

H

(r − d

(K∑

j=1

θjBj (·),�γnn

))), (3.4)

(cf. with (2.5)). The set �γnn is defined by

�γnn =

L⋃=1

Gγn(W,V) (3.5)

where for each = 1, . . . ,L, Gγn(W,V) is a 100(1 − γn)% confidence ellipsebased on the n readings for the th point (W,V) defined as the ellipse formedby the points (x, y) that satisfy the equation

n((W,V) − (x, y))t�−1

((W,V) − (x, y)

) ≤ χ22 (γn)

where W and V are the sample average of the vectors W and V respectively,χ2

2 (γn) is the 100γn-percentile of the chi-square distribution with 2 degrees offreedom. These ellipses are centered at the sample mean (W,V) and have axes

(

√χ2

2 (γn)λ,j /ne,j ,−√

χ22 (γn)λ,j /ne,j ), where λ,j , j = 1,2, are the eigenval-

ues of � and e,j are the corresponding eigenvectors.

530 R. Dias et al.

Here, �γnn can be thought as a fattening of the averaged point process η̄ =

1n(η1 + · · · + ηn) to account for the variability due to measurement errors. The confi-

dence limits 1 − γn will be chosen suitably in order to get strong convergence of theestimators (Theorem 3.2).

Remark If the covariance matrices � are unknown, the points (x, y) belonging tothe confidence ellipses have to satisfy

n((W,V) − (x, y)

)tS−1 ((W,V) − (x, y)) ≤ 2(n − 1)

(n − 2)F2,n−2(γn)

where S is the standard sample covariance of the vectors W and V and F2,n−2(γn)

is the 100γn-percentile of the F distribution with (2, n − 2) degrees of freedom.

Lemma 3.1 If∑

1 − (1 − γn)L < ∞ then

P(N ⊂ �γnn i.o.) = 0. (3.6)

Proof Since the processes η1, . . . , ηL are independent, by the definition of confidenceellipses we have

P(N ⊂ �γnn ) =

L∏=1

P((w, v) ∈ Gγn(W,V)) = (1 − γn)L.

Thus, the result follows immediately from the Borel–Cantelli Lemma. �

Lemma 3.2

(a) {(W̄, V̄), = 1, . . . ,L} → N, a.s.(b) If χ2

2 (γn)/n → 0 then �γnn → N, a.s. as n → ∞.

(c) For fixed θ ∈ RK we have

Qα,ψ,r,H,n(θ) → Qα,ψ,r,H (θ), a.s. (3.7)

as n → ∞.

Proof Part (a) follows directly from the Strong Law of Large Numbers for i.i.d. ran-dom vectors.

To prove (b) we just have to notice that the set �γnn is the union of ellipses with

centers at the points of N and axes having length 2√

χ22 (γn)λ,j /n.

Part (c) is directly implied by (a) and (b). �

Notice that it is possible to satisfy simultaneously the conditions of Lemmas 3.1and 3.2. By Chernoff inequality, for any random variable Z we have

P(Z > a) ≤ e−atE(eZt ), for all t.

A penalized nonparametric method for nonlinear constrained 531

Thus taking Z ∼ χ22 we obtain

γn ≤ e−χ22 (γn)t

(1

1 − 2t

).

From now on, to simplify the notation we will drop the subscript (α,ψ, r,H).

Lemma 3.3 For any continuous functions f : [0, b] → R and g : [0, b] → R and� ⊂ R

2 we have

supx∈[0,b]

|f (x) − g(x)| ≥ |d(f,�) − d(g,�)|.

Proof In fact, let ε = supx∈[0,b] |f (x) − g(x)| and without loss of generality assumed(f,�) > d(g,�). It is easy to see that for any x ∈ [0, b] and w ∈ �

d((x,f (x)),w) ≤ |f (x) − g(x)| + d((x, g(x)),w).

Therefore,

d((x,f (x)),�) ≤ |f (x) − g(x)| + d((x, g(x)),�

)≤ ε + d

((x, g(x)),�

).

The result follows immediately by taking infimum over x ∈ [0, b] in the last ine-quality. �

Theorem 3.1 The functions Qn and Q are continuous functions.

Proof We just need to check that the map

θ �→ �

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),�)))

is continuous for any set � ⊂ R2. In fact, this map is Lipschitz. To see this, take

θ,φ ∈ RK

∣∣∣∣∣�(

Zα + √H

(r − d

(K∑

j=1

θjBj (·),�)))

− �

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),�)))∣∣∣∣∣

=∣∣∣∣∣�′(ξ)

√H

(d

(K∑

j=1

θjBj (·),�)

− d

(K∑

j=1

φjBj (·),�))∣∣∣∣∣

532 R. Dias et al.

≤ 1√2π

√H sup

z∈[0,b]

∣∣∣∣∣K∑

j=1

(θj − φj )Bj (z)

∣∣∣∣∣≤ C|θ − φ|

for a suitable positive constant C. The equality follows from the Mean Value The-orem, for some ξ ∈ R. The first inequality follows from Lemma 3.3 and �′(ξ) =(1/

√2π) exp(−ξ2/2). �

To prove the main theorem, it is important to notice that Qn is a random func-tion since it depends upon the random processes ηi = {(W1,i , V1,i ), . . . , (WL,i,VL,i)}for i = 1, . . . , n through the confidence ellipse �

γnn . In the following proof, when

needed, we emphasize the fact that we are dealing with a specific realization ω writ-ing Qn(θ,ω) and �

γnn (ω).

Lemma 3.4 For any φ ∈ RK we have

P(Qn(φ) < Q(φ) i.o.) = 0. (3.8)

Proof From Lemma 3.1 we have that the event A = [N ⊂ �γnn i.o.] has probability

zero. This means that for any ω ∈ Ac there exists n0(ω) with

N ⊂ �γnn (ω)

for all n ≥ n0.Therefore, for ω ∈ Ac and any φ ∈ R

K we have

d

(∑j

φjBj ,�γnn (ω)

)≤ d

(∑j

φjBj ,N

)(3.9)

for all n ≥ n0. Thus, for any choice of the tuning parameters,

ψ�

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),�γnn

)))

≥ ψ�

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),N)))

since � is strictly increasing, ψ,H > 0. Hence, for all ω /∈ A

Qn(φ,ω) ≥ Q(φ), (3.10)

for all n ≥ n0. Equation (3.8) follows immediately. �

We now can state the main theorem of this paper.

A penalized nonparametric method for nonlinear constrained 533

Theorem 3.2 The solution of (3.3), θ̂n, is a strongly consistent estimator for θ , thesolution of (2.5).

Proof In this case, we need the concept of epiconvergence. We need to prove that Qn

epiconverges to Q as n → ∞. The following result is true: if θn is an εn minimizerof Qn with εn → 0, then any convergent sub-sequence of {θn} must converge to apoint θ which minimizes Q and the optimal value Qn(θn) must also converge to theminimal point Q(θ). Notice that there is no need for uniqueness of the minimizers.If Q has a unique minimizer θ then θ is the only accumulation point of the sequence{θn}. Also, it does not guarantee that θ is finite. There are several characterizations ofepiconvergence, we follow [3]. The sequence Qn epiconverges to Q if:

Q(θ) ≤ supB∈N (θ)

lim infn→∞ inf

φ∈B{Qn(φ)}, (3.11)

Q(θ) ≥ supB∈N (θ)

lim supn→∞

infφ∈B

{Qn(φ)} (3.12)

where N (θ) denotes the set of neighborhoods of the point θ .Notice that there exists a countable base B = {B1,B2, . . .} for the topology of R

K .For any point θ , let

Nc(θ) = B ∩ N (θ).

Then, in our case, the suprema over uncountable set N (θ) in (3.12) and (3.11) canbe replaced by suprema over the countable set Nc(θ).

First we will prove (3.12). If B ∈ B and θ ∈ B ∈ Nc(θ) then

Q(θ) = limn→∞Qn(θ) = lim sup

m→∞Qm(θ) ≥ lim sup

m→∞infφ∈B

Qm(φ)

since by Corollary 3.2

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),�γnn

)))

→ �

(Zα + √

H

(r − d

(K∑

j=1

φjBj (·),N)))

,

almost surely, as n → ∞. Therefore,

Q(θ) ≥ supB∈Nc(θ)

lim supn

infB∈B

Qn(φ)

and (3.12) is proved.For (3.11), first we choose a countable dense set �c = {θ1, θ2, . . .} as follows. For

each n, let θn ∈ Bn such that

Q(θn) ≤ infφ∈Bn

Qn(φ) + 1

n.

Therefore,

534 R. Dias et al.

supBn∈Nc

lim infm→∞ inf

φ∈Bn

Qm(φ) ≥ supBn∈Nc

infφ∈Bn

Q(φ) ≥ supBn∈Nc

{Q(θn) − 1

n

}≥ Q(θ)

where the first inequality follows from Lemma 3.4. �

4 Numerical examples for complete vision

In this section, we will use the language of the path finding for an autonomousvehicle. We performed numerical simulations for a large variety of obstacle fields. Forillustration purposes, we exhibit only the cases where we have one and ten readingsfor each obstacle.

In all the plots the black points are the real obstacles, the crosses are mean ofthe observed obstacles, the confidence ellipses curves are drawn in gray. The solidcurve is the curve we would obtain by the procedure described in Sect. 2 if we had nomeasurement error. The dashed and dash-point curves are the ones obtained by (3.3).

(a)

(b)

Fig. 6 Estimated trajectories for independent case: deterministic trajectory (solid curve), trajectoriesbased on one and ten observations (dashed line, dash-point line). The errors are normally distributed with(a) ρ = 0.6, σ,1 ∼ U(0, (.8/30)W) and σ,2 ∼ U(0, (.6/30)W), (b) ρ = −0.7, σ,1 ∼ U(0, (.7/30)W)

and σ,2 ∼ U(0, (.5/30)W)

A penalized nonparametric method for nonlinear constrained 535

Figures 6 and 7 show that in the presence of random variability, the paths tend tostay further from the obstacle, but as the number of reading increases it will convergeto the deterministic trajectory.

Figure 8 presents two examples where the obstacle field is highly structured andexhaustive search-based methods have to analyze several unfeasible trajectories. Onthe other hand, these type of obstacle fields do not pose any extra complexity orincrease in the running time for our algorithm.

As pointed before, the procedure is not limited to avoiding circular sets. The fol-lowing two examples shows the versatility of the procedure.

Non-circular threat zones Figure 9 (cf. Fig. 7) presents the solution of the deter-ministic problem for diamond type sets.

Race corridors Sometimes the vehicles or planes have to stay inside a prescribedarea or road. Moreover, there could be checkpoints along the race. This can be seen,for example, in the DARPA Grand Challenge. Figure 10 shows an example of two

Fig. 7 Estimated trajectoriesfor iid case: deterministictrajectory (solid curve),trajectories based on one and tenobservations (dashed line,dash-point line). The errors arenormally distributed with(a) σ1 = 0.1, σ2 = 0.2 andρ = −0.8 and (b) σ1 = 0.07,σ2 = 0.15 and ρ = 0.6

(a)

(b)

536 R. Dias et al.

(a) (b)

Fig. 8 Estimated trajectories for iid case: deterministic trajectory (solid curve), trajectories based on oneand ten observations (dashed line, dash-point line). The errors are normally distributed with (a) σ1 = 0.3,σ2 = 0.4 and ρ = 0.8 and (b) σ1 = 0.3, σ2 = 0.2 and ρ = 0.7

roads having in their intersection a smaller circle that is a checkpoint, that is, thevehicle has to stay inside the roads avoiding the obstacles and forced to pass insidethe inner circle.

5 Partial vision

In the previous sections, we assumed that the whole point processes and consequentlythe set � is known in advance. Suppose, however, that we would like to construct thetrajectory stepwisely at the points 0 < x1 < x2 < · · · < xN < b and at each “check-point” a new � is presented. In terms of the applications, maybe the obstacle fieldcannot be seen from the starting point and the strategy cannot be computed beforeleaving. In fact, it is more realistic to imagine that the sensors have a finite range R

smaller than the total field. In this case, a sequential procedure is necessary.For (u, v) ∈ R

2 denote

Su,v = {(x, y) : 0 < x − u ≤ R}.The proposed algorithm to estimate the best trajectory is:

1. Let N1 be the point process N restricted to S(0,0). Let f̂1 ∈ HK be the minimizerof the cost function

A penalized nonparametric method for nonlinear constrained 537

Fig. 9 Estimated trajectoriesfor deterministic trajectory fordiamond shape obstacles

(a)

(b)

∫ B

0

√1 + f ′

1(t)2dt + λ�

(Zα + √

n(r − d(f1,N1)))

(5.1)

where f1(0) = 0, f1(b) = 0.2. Given the solution f̂i−1 at step i − 1, let Ni be the point process N restricted to

S((i−1)R,0). Let f̂i ∈ HK be the minimizer of

∫ B

0

√1 + f ′

i (t)2dt + λ�

(Zα + √

H(r − d(fi,Ni)

)), (5.2)

subject to f(ν)i ((i − 1)R) = f̂

(ν)i−1((i − 1)R), for ν = 0,1,2, fi(b) = 0.

This sequential procedure can be interpreted as: the vehicle “thinks” at step i thatthere is no obstacles after distance R. When it arrives at checkpoints R,2R, . . . it

538 R. Dias et al.

Fig. 10 Trajectory constrained to avoid obstacles, to follow a road and to pass through one checkpointat (8, 7.5)

restarts the procedure joining smoothly the paths. Another important remark is that,at step i the procedure is not exactly as it was in the beginning because the vehiclewill be at a point (iR,yi), for some yi not necessarily 0.

Notice that this estimation was described considering the deterministic case. Thestochastic procedure can be easily implemented in this case.

Figure 11 show the simulated results for this case. Figure 11(b) raises a very im-portant issue that is to be addressed in a future paper. Since the trajectory is computedat points x1, x2, . . . , xN , this can be interpreted as the vehicle only reloads after run-ning the whole R distance. Since the path has to be smooth, if there is a point veryclose to the line xi the vehicle when it gets there it will not deviate from the obsta-cle and crashes. To fix this problem, the vehicle should reload at shorter steps andthe procedure redone. In this case, a very interesting scenario appears. Assume thatthe range of vision of the vehicle is R, it reloads after steps of size R/2m and themeasurement errors are more precise as the distance between the sensors and the ob-stacles decrease, how to combine different readings, for example from point 0, pointR/2m, R/2m−1, etc.?

6 Conclusion

In this work our attention was focused on the presentation of an alternative algorithmto find the shortest route between two points constrained to avoid and/or intersect

A penalized nonparametric method for nonlinear constrained 539

(a)

(b)

Fig. 11 The solid curve is the goal path estimated using (2.5), the dashed and dash-point curves aresequentially estimated reloading stepwisely at the gray vertical lines. The errors are normally distributedwith com (a) ρ = 0.5, σ,1 ∼ U(0, (.6/30)W), σ,2 ∼ U(0, (.4/30)W) and reload at steps of size 6(b) ρ = −0.6, σ,1 ∼ U(0, (.5/30)W), σ,2 ∼ U(0, (.6/30)W) and reload at steps of size 15

some prescribed sets. The novelty of our methodology is that the penalized non-parametric approach not only reduces the dimension of the problem but also hasgreat flexibility to incorporate new factors:

(i) it naturally transforms a constrained problem to an unconstrained one;(ii) it transforms an infinite dimensional problem to a finite-dimensional one, and

only few coefficients are necessary to represent the trajectory;(iii) its solution is already a smooth curve with degree of smoothness adapted to the

problem;(iv) it can deal with any shape for the avoidance sets as well as allowing to force the

path to remain inside certain boundaries;(v) it can treat the case of independent measurement errors in detecting the obstacles

providing a consistent stochastic solution.

We think that our adaptive obstacle avoidance strategies can be built efficientlyto incorporated more realistic assumptions. For example, for partial vision, it would

540 R. Dias et al.

be natural to consider the case that after each step of size R/2m, the sensors makenew readings from the obstacle field through procedures dependent on the previousreadings. In the simplest case it would be a Markovian dependence.

Realistic complex problems can only be solved through decomposition intosmaller and simpler pieces. This paper is a contribution to the analysis of the latter.

Acknowledgements We thank Richard Murray and Timothy Chung for introducing us to this problemand for many fruitful discussions. Also, Mario Martinez helped us discussing the problem. R.D. thanksthe hospitality and enlightening environment provided by Caltech during his visit. We would like to thankthe associate editor and the anonymous referee for valuable suggestions that greatly improved this paper.This paper was partially supported by CNPq Grants 301058/2004-0 (RD) and 302279/2004-0 (NG) andFAPESP Grant 2006/02095-5 (AZ).

References

1. Antoniadis, A.: Wavelet methods for smoothing noisy data. In: Wavelets, Images, and Surface Fitting,Chamonix-Mont-Blanc, 1993, pp. 21–28. A.K. Peters, Wellesley (1994)

2. Asseo, S.J.: In-flight replanning of penetration routes to avoid threat zones of circular shapes. In:Proceedings of the IEEE 1998 National Aerospace and Electronics Conference (NAECON 1998),pp. 383–391 (1998)

3. Attouch, H.: Variational Convergence for Functions and Operators. Applicable Mathematics Series,Pitman Advanced Publishing Program. Boston, Pitman (1984)

4. Barraquand, J., Latombe, J.-C.: Nonholonomic multibody mobile robots: controllability and motionplanning in the presence of obstacles. Algorithmica 10(2–4), 121–155 (1993). (Computational robot-ics: the geometric theory of manipulation, planning, and control)

5. Bodin, P., Villemoes, L.F., Wahlberg, B.: Selection of best orthonormal rational basis. SIAM J. ControlOptim. 38(4), 995–1032 (2000) (electronic)

6. Choset, H., Lynch, K., Hutchinson, S., Kantor, G., Burgardand, W., Kavraki, L., Thrun, S.: Principlesof Robot Motion: Theory, Algorithms and Implementations. MIT Press, Cambridge (2005)

7. Cremean, L.B., Foote, T.B., Gillula, J.H., Hines, G.H., Kogan, D., Kriechbaum, K.L., Lamb, J.C.,Leibs, J., Lindzey, L., Rasmussen, C.E., Stewart, A.D., Burdick, J.W., Murray, R.M.: ALICE: Aninformation-rich autonomous vehicle for high-speed desert navigation. J. Field Robotics 23(9), 777–810 (2006)

8. de Boor, C.: A Practical Guide to Splines. Springer, New York (1978)9. De Vore, R., Petrova, G., Temlyakov, V.: Best basis selection for approximation in Lp . Found. Com-

put. Math. 3(2), 161–185 (2003)10. Dias, R.: Density estimation via hybrid splines. J. Stat. Comput. Simul. 60, 277–294 (1998)11. Dias, R.: Sequential adaptive non parametric regression via H-splines. Commun. Stat. Comput. Simul.

28, 501–515 (1999)12. Dias, R.: A note on density estimation using a proxy of the Kullback–Leibler distance. Brazilian J.

Probab. Stat. 13(2), 181–192 (2000)13. Laumond, J.-P. (ed.): Robot Motion Planning and Control. Lecture Notes in Control and Information

Science, vol. 229. Springer, New York (1998). Available online: http://www.laas.fr/~jpl/book.html14. Grundel, D., Murphey, R., Pardalos, P., Prokopyev, O. (eds.): Cooperative Systems, Control and Op-

timization. Lecture Notes in Economics and Mathematical Systems, vol. 588. Springer, New York(2007)

15. Hirsch, M.J., Pardalos, P., Murphey, R., Grundel, D. (eds.): Advances in Cooperative Control and Op-timization. Lecture Notes in Control and Information Sciences, vol. 369. Springer, New York (2008).Papers from a meeting held in Gainesville, FL, 31 January–2 February 2007

16. Jabri, Y.: The Mountain Pass Theorem: Variants, Generalizations and Some Applications. CambridgeUniversity Press, Cambridge (2003)

17. Kohn, R., Marron, J.S., Yau, P.: Wavelet estimation using Bayesian basis selection and basis averaging.Stat. Sinica 10(1), 109–128 (2000)

18. Kooperberg, C., Stone, C.J.: A study of logspline density estimation. Comput. Stat. Data Anal. 12,327–347 (1991)

A penalized nonparametric method for nonlinear constrained 541

19. Lavalle, S.: Planning Algorithms. Cambridge University Press, Cambridge (2006)20. Luo, Z., Wahba, G.: Hybrid adaptive splines. J. Am. Stat. Assoc. 92, 107–116 (1997)21. Reif, U.: Orthogonality of cardinal B-splines in weighted Sobolev spaces. SIAM J. Math. Anal. 28(5),

1258–1263 (1997)22. Silverman, B.W.: Density Estimation for Statistics and Data Analysis. Chapman and Hall, London

(1986)23. Tiwari, A., Chandra, H., Yadegar, J., Wang, J.: Constructing optimal cyclic tours for planar exploration

and obstacle avoidance: A graph theory approach. In: Advances in Variable Structure and SlidingMode Control. Springer, Berlin (2007)

24. Vidakovic, B.: Statistical Modeling by Wavelets. Wiley Series in Probability and Statistics: AppliedProbability and Statistics. Wiley-Interscience, New York (1999)


Top Related