a penalized nonparametric method for nonlinear constrained optimization based on noisy data

Download A penalized nonparametric method for nonlinear constrained optimization based on noisy data

Post on 15-Jul-2016




0 download

Embed Size (px)


  • Comput Optim Appl (2010) 45: 521541DOI 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 Aand 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 Estatstica, 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: dias@ime.unicamp.br

    N.L. Garciae-mail: nancy@ime.unicamp.br

    A.Z. Zambome-mail: adrzzz@yahoo.com

  • 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 R2, 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 fbelonging 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 R2 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) = 0

    and 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


    (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



    (1 + f (t))2dt + J (f ) (2.1)


    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 H22.

    For the sake of simplicity in notation, from now on we will consider that wehave L points in R2 with coordinates i = (wi, vi), i = 1, . . . ,L. Denote by N ={1, 2, . . . , L} and


    i=1B(i, r)

    where B(, r) = {z R2 : d(z, ) < r} and d is the Euclidean distance. It is easy tosee 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 penalizationJ (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 d