Published on

10-Mar-2017View

213Download

1

Transcript

<ul><li><p>IEEE TRANSACTIONS ON ROBOTICS, VOL. 25, NO. 2, APRIL 2009 353</p><p>Extending the Limits of Feature-Based SLAMWith B-Splines</p><p>Luis Pedraza, Member, IEEE, Diego Rodriguez-Losada, Member, IEEE, Fernando Mata, Member, IEEE,Gamini Dissanayake, Member, IEEE, and Jaime Valls Miro</p><p>AbstractThis paper describes a simultaneous localization andmapping (SLAM) algorithm for use in unstructured environmentsthat is effective regardless of the geometric complexity of the envi-ronment. Features are described using B-splines as modeling tool,and the set of control points defining their shape is used to form acomplete and compact description of the environment, thus mak-ing it feasible to use an extended Kalman-filter (EKF) based SLAMalgorithm. This method is the first known EKF-SLAM implemen-tation capable of describing general free-form features in a para-metric manner. Efficient strategies for computing the relevant Ja-cobians, perform data association, initialization, and map enlarge-ment are presented. The algorithms are evaluated for accuracy andconsistency using computer simulations, and for effectiveness usingexperimental data gathered from different real environments.</p><p>Index TermsKalman filtering, mobile robots, simultaneous lo-calization and mapping (SLAM), spline functions.</p><p>I. INTRODUCTION</p><p>ONE of the current key challenges of the simultaneouslocalization and mapping (SLAM) problem is the devel-opment of appropriate parameterizations to represent environ-ments of increasing complexity. While a substantial body ofliterature exists in methods for representing unstructured envi-ronments, most of them are not suitable for use in one of thecommon frameworks developed during the past decades, as itis the solution based on an extended Kalman filter (EKF). Forexample, the use of occupancy grids [1], based on dividing theenvironment into small cells of predefined size, and classifyingthem as occupied or not, and its many variants, would result inan impracticably large state vector. The importance and effec-tiveness of these techniques are undeniable, but we propose adifferent approach to the problem.</p><p>Much of the early SLAM work relied on simple point featuresfor describing the environment [2][4]. While this approach</p><p>Manuscript received December 17, 2007. First published February 18, 2009;current version published April 3, 2009. This paper was recommended for pub-lication by Associate Editor G. Dudek and Editor L. Parker upon evaluation ofthe reviewers comments. This work was supported in part by the DireccionGeneral de Investigacion of Spain under Project Robonauta DPI2007-66846-C02-01. The work of L. Pedraza was supported by the Consejera de-Educacionde-la-Comunidad de-Madrid under for the Ph.D. scholarship.</p><p>L. Pedraza is with the Intelligent Control Group, Universidad Politecnica deMadrid, E-28006 Madrid, Spain (e-mail: luis.pedraza@ieee.org).</p><p>D. Rodriguez-Losada and F. Mata are with the Intelligent Control Group,Universidad Politecnica de Madrid, E-28006 Madrid, Spain.</p><p>G. Dissanayake and J. V. Miro are with the School of Electrical, Mechanical,and Mechatronic Systems, University of Technology, Sydney, N.S.W., Australia.</p><p>The paper has supplementary downloadable material available at http://ieeexplore.ieee.org, provided by the authors. These videos shows two exper-iments of SLAM with B-splines with data gathered from real environments.This material is 9.5 MB and 112 MB in size.</p><p>Color versions of one or more of the figures in this paper are available onlineat http://ieeexplore.ieee.org.</p><p>Digital Object Identifier 10.1109/TRO.2009.2013496</p><p>simplifies the formulation of the SLAM estimator, two maindisadvantages arise when relying solely on this representation.The first and obvious problem emerges when the environmentdoes not have sufficient structure to robustly extract point fea-tures; for example, in an underground mine [5]. The second andmore significant issue is the fact that only a small fraction ofinformation available from popular sensors, such as laser rangefinders, is exploited. Much of the data that do not correspond tothe expected features are discarded.</p><p>A number of SLAM algorithms that do not use use a specificgeometric model have emerged recently. Here, the completerobot trajectory is present in the state vector, and the raw sensorinformation from different robot poses is processed to obtain anaccurate relationship between these poses [6]. Equivalent ver-sions of FastSLAM that exploit complete laser scans have alsobeen presented [7], [8]. While these strategies have been suc-cessfully used to generate accurate visualizations of complexstructures and detailed maps of the environments, they cannotexploit the inherent information gain available in feature-basedSLAM, where map quality is increased due to precise mathemat-ical description of features, stochastic information relating all ofthem is available, and the interpretability of the map is increased.</p><p>Several strategies for incorporating a larger fraction of theinformation gathered by the sensors, using more complex geo-metric primitives, have recently been reported. The use of linesegments [9][11] and polylines [12], while promising, raisesome issues related to the consistency of the solution and re-quire further work. While segment-based solutions have beensuccessful in typical indoor environments, the increasing pres-ence of curved geometries, more popular every day in modernconstructions, can create significant problems. In particular, at-tempting to interpret information using an incorrect geometricmodel is one of the major causes of failure of many estimation al-gorithms. Some efforts have been made when circle features areavailable [13], but that is still a major simplification. Thus, moregeneric representations of the environment can potentially im-prove the robustness, effectiveness, and reliability of the SLAMimplementations.</p><p>A hybrid strategy consists of modeling the region surround-ing a point feature using a shape model in a coordinate frameattached to the feature [14], allowing the point features map tobe updated while the shape models are only used to increase theinformation content of the observation. A full parameterizationthat captures all the information available in the observations,able to be updated in a statistically consistent manner, remainsan interesting challenge.</p><p>Recently, we presented BS-SLAM, a novel feature-basedsolution to the SLAM problem, based on the utilization of</p><p>1552-3098/$25.00 2009 IEEE</p></li><li><p>354 IEEE TRANSACTIONS ON ROBOTICS, VOL. 25, NO. 2, APRIL 2009</p><p>Fig. 1. Examples of cubic splines ( = 4) and their corresponding basis functions using (a) a clamped knot vector (c : 0 = = 3 . . . 6 = = 9 )and (b) an unclamped knot vector (u : 0 9 ). Knots locations are represented as circles and control points as squares. The dashed blue line is the splinecontrol polygon, which joins all the control points.</p><p>B-spline curves to represent the boundary between occupied andunoccupied regions in complex environments [15]. B-splinesprovide naturally compact descriptions for both straight andcurved geometries, consisting of a set of control points defin-ing their shape. These points, grouped in a state vector, whichconveys no angular information for the static elements of themap, fully describe the environment. Computationally efficientstrategies for: 1) initializing and extending the state vector; 2)formulating a suitable observation equation; and 3) evaluationof appropriate Jacobians for easy implementation of EKF equa-tions, were introduced in [15].</p><p>This paper extends those results presenting additional experi-mental evidences, using both real and simulated data, making anemphasis on the importance of some critical aspects: 1) a studyof the impact of the defining elements of B-spline curves on theexpected performance of the algorithm; 2) providing some gen-eral guidelines to adequately choose the parameters involved inthe segmentation process; and 3) a deep consistency analysis,showing the limitations of the algorithm, as occurring with anyother EKF-SLAM implementation [16].</p><p>The paper is organized as follows. Section II introduces fun-damental concepts regarding the extensive theory of B-splinecurves. Section III shows how these powerful tools naturally fitinto the EKF-SLAM framework. Finally, extensive experimen-tal results and conclusions are presented in Sections IV and V.</p><p>II. BASIC THEORY OF SPLINES</p><p>In this section, some fundamental concepts of the B-splinestheory are presented. The term spline refers to a wide class offunctions that are used in a range of technical and scientificapplications, where interpolation or smoothing of noisy data ina flexible yet computationally efficient way is required. A splineof degree (order 1) is a piecewise polynomial curve, i.e., acurve composed of several polynomial pieces of degree . Theirmost common representation is based on the linear combinationof a special type of basis functions, known as B-spline basisfunctions.</p><p>A. B-Splines Definition</p><p>Letting s (t) be the position vector along an m-dimensionalcurve as a function of the parameter t , a spline curve oforder , with control points xi m (i = 0, . . . , n) and knotvector = {0 , . . . , n+k} can be expressed as</p><p>s (t) =n</p><p>i=0</p><p>xii, (t) (1)</p><p>where i, (t) are the normalized B-spline basis functions of or-der . These functions are defined by the Cox-de Boor recursionformulas [17], [18]</p><p>i,1 (t) ={1, if i t < i+10, otherwise (2)</p><p>and for all > 1</p><p>i, (t) =(t i)</p><p>i+1 ii,1 (t) +</p><p>(i+ t)i+ i+1</p><p>i+1,1 (t) .</p><p>(3)The knot vector is any nondecreasing sequence of real num-</p><p>bers (i i+1 for i = 0, . . . , n+ 1), and its structure canlie in two different categories: clamped, when the multiplicity ofthe extreme knot values is equal to the order of the curve, andunclamped when this does not occur [17], [19]. When clampedknot vectors are used, first and last control points are coincident,respectively, with the beginning and end of the spline curve, asFig. 1 illustrates.</p><p>B. Properties of Spline Curves</p><p>B-splines exhibit many interesting mathematical and geomet-rical properties. For their special significance in the followingdiscussions, some of them are enumerated here.</p><p>1) The maximum order of the curve equals the number ofcontrol points. Equivalently, the minimum number of con-trol points equals the order of the curve. This means, forexample, that a cubic B-spline requires at least four controlpoints to be defined.</p></li><li><p>PEDRAZA et al.: EXTENDING THE LIMITS OF FEATURE-BASED SLAM WITH B-SPLINES 355</p><p>2) The curve generally follows the shape of the controlpolygon.</p><p>3) Any affine transformation is applied to the curve byapplying it to the control polygon.</p><p>4) Each basis function i, (t) is a piecewise polynomial oforder with breaks i, . . . , i+ that vanish outside theinterval [i, i+) and is positive on the interior of thatinterval</p><p>i,(t) > 0 i t < i+ . (4)5) As a consequence of property 4), the value of s (t) at</p><p>a given parameter location j t j+1 for some j { 1, . . . , n} depends only on of the coefficients</p><p>s (t) =j</p><p>i=j+1xii, (t) . (5)</p><p>6) When clamped knot vectors are used, the sum of all theB-spline basis functions for any value of the parameter tis 1, i.e.</p><p>ni=0</p><p>i, (t) = 1. (6)</p><p>7) The derivative of a spline of order is a spline of one order 1. The control points of the derived function can beobtained by differencing the original ones [18]</p><p>ds (t)dt</p><p>= s (t) = ( 1)n</p><p>i=0</p><p>xi xi1i+1 i</p><p>i,1 (t )</p><p>(7)For further information and justification of these properties,</p><p>see [17][20].</p><p>C. Curve Fitting</p><p>One of the greatest appeals of splines curves is their abilityfor approximating noise-contaminated data. In this section, weconsider the problem of obtaining a spline curve that fits a set ofdata points dj , j = 0, . . . ,m. If a data point lies on the splinecurve, then (1) must be satisfied</p><p>dj = 0, (tj )x0 + + n, (tj )xn , j = 0, . . . ,m.This system of equations can be more compactly written as</p><p>d = Bx</p><p>d = [d0 d1 . . . dm ]T</p><p>x = [x0 x1 . . . xn ]T</p><p>B =</p><p>0, (t0) . . . n, (t0)</p><p>.... . .</p><p>...0, (tm ) . . . n, (tm )</p><p> .</p><p>(8)</p><p>MatrixB is usually referred to as the collocation matrix. Eachof its rows has at most nonnull values [recall property 4)]. Theparameter value tj defines the position of each data point djalong the curve, and can be approximated by the cumulatedchord length between consecutive data points</p><p>t0 = 0</p><p>tj =j</p><p>s=1</p><p>ds ds1 , j 1</p><p> (9)</p><p>where is the Euclidean norm. The total length of the curveis</p><p> =ms=1</p><p>ds ds1 (10)</p><p>which is taken as the maximum value of the knot vector.When fitting noisy data acquired by a laser range finder,</p><p>the most general case occurs for 2 n+ 1 < m+ 1; theproblem is overspecified and a least-squares solution can beobtained using the pseudoinverse matrix of B</p><p>x =[BT B</p><p>]1BT d = d. (11)</p><p>If the order of the spline curve is predefined, the numberof control points n+ 1 (or equivalently, the number of knotsn+ + 1) and the parameter values along the curve are knownas indicated in (9), then the basis functions i, (tj ), and hence,the matrix B can be obtained.</p><p>In BS-SLAM, clamped knot vectors are generated taking thetotal length of the curve and defining a knot spacing , whichdepends on the complexity of the environment. Recall that knotsare the joints of the individual polynomial pieces that a splineis composed of, so complex environments containing objectswith high curvatures need a small spacing (high knot density),while straight features can be described properly using longerpolynomial pieces (lower knot density).</p><p>To show the effects of both the curve order and the knotspacing on the quality of the fitting, we performed experimentswith a simulated static robot in front of a curved wall. Thisunique feature was described by the polar equation</p><p>z = 5 + 0.5 sin (5) (12)</p><p>where z is the distance in meters from the curve to the originand is the polar angle. The simulated range-bearing sensorhad a forward-facing 180 field-of-view and maximum range of8 m. For each measure across the angular range of the sensor,with a resolution of 1, synthetic noise with normal distributionN (0, L = 5 mm) was added to the ideal measure computedusing (12).</p><p>Fig. 2 shows the effect of varying the knot spacing used inthe data fitting with a cubic spline. For each experiment, theaverage value of theMSE over 50 Monte Carlo experimentswas computed. The residuals are the differences between thereal distance from the sensor to the wall and the distance fromthe sensor to the obtained spline for the same laser beam. It isclear that, as the number of polynomial pieces increases, theapproximation is better.</p><p>Fig. 3 shows similar experiments for different orders of thespline used in the fitting process. Here, the knot spacing isconstant ( = 2.92 m). The average MSE over 50 runs of aMonte Carlo simulation is clearly smaller for the cubic splinegiven a fixed knot density. Better results could be obtained forthe quadric spline by decreasing the knot density. However, foreach additional knot, a new control point should be...</p></li></ul>