Robust Range-Only Mapping via Sum-of-Squares Polynomials

Proceedings of the 20th World Congress Proceedings of the 20th World Congress Proceedings of the the 20th World World Congress The International Federation of Congress Automatic Control Proceedings of 20th The International Federation of Congress Automatic Control Proceedings of the 20th9-14, World The Federation of Control Toulouse, France, July 2017 The International International Federation of Automatic Automatic Control Available online at www.sciencedirect.com Toulouse, France, July 9-14, 2017 The International Federation of Automatic Control Toulouse, France, July 9-14, 2017 Toulouse, France, July 9-14, 2017 Toulouse, France, July 9-14, 2017

ScienceDirect

IFAC PapersOnLine 50-1 (2017) 11491–11497

Robust Range-Only Mapping via Robust Range-Only Mapping via Robust Range-Only Mapping via Robust Range-Only Mapping via Sum-of-Squares Polynomials Sum-of-Squares Polynomials Sum-of-Squares Polynomials Sum-of-Squares Polynomials ∗ ∗

∗ ´ G´ e nev´ e Laroche ∗ Adlane Habed ∗ Edouard ∗ ´ G´ e nev´ e Habed Laroche ∗ ∗ ∗ ´ ∗ Adlane ∗ Edouard ∗ ∗∗ ´ G´ e nev´ e Adlane Habed Edouard Laroche G´ enev´ e Olivier Adlane Habed Edouard Laroche Kermorgant ∗∗ ∗ ∗ ∗ ´ Kermorgant ∗∗ G´ enev´ e Olivier Adlane Habed Edouard Laroche ∗∗ Olivier Kermorgant Kermorgant ∗∗ Olivier Olivier Kermorgant ∗ ∗ ICube Laboratory, University of Strasbourg-CNRS, Strasbourg, France ∗ ICube Laboratory, University of Strasbourg-CNRS, Strasbourg, France ∗ ICube Laboratory, University of Strasbourg-CNRS, Strasbourg, France France University of Strasbourg-CNRS, Strasbourg, (e-mails: {l.geneve, laroche, habed}@unistra.fr) ∗ ICube Laboratory, (e-mails: {l.geneve, laroche, habed}@unistra.fr) ICube∗∗Laboratory, University of Strasbourg-CNRS, Strasbourg, France (e-mails: {l.geneve, laroche, habed}@unistra.fr) ´ (e-mails: {l.geneve, laroche, habed}@unistra.fr) LS2N, Ecole Centrale de Nantes, Nantes, France ∗∗ ´ (e-mails: {l.geneve, laroche, habed}@unistra.fr) Centrale de ∗∗ LS2N, ´ ∗∗ ´ LS2N, Ecole Ecole Centrale de Nantes, Nantes, Nantes, Nantes, France France Ecole Centrale de Nantes, Nantes, France [email protected]) ∗∗ LS2N,(e-mail: ´ [email protected]) LS2N,(e-mail: Ecole Centrale de Nantes, Nantes, France (e-mail: (e-mail: [email protected]) [email protected]) (e-mail: [email protected]) Abstract: This work presents new approach for mapping static beacons given only range Abstract: This This work work presents presents aa a new new approach approach for for mapping mapping static static beacons beacons given given only only range range Abstract: Abstract: This work presents a new approach for mapping static beacons given only range measurements. An original formulation using sum-of-squares and linear matrix inequalities is measurements. An original formulation using sum-of-squares and linear matrix inequalities is Abstract: This work presents a new approach for mapping static beacons given only range measurements. An original formulation using sum-of-squares sum-of-squares and linear linear matrix inequalities inequalities is measurements. An original formulation using and matrix is derived to test if a measurement is inconsistent with a bounding box containing the beacon derived to test if a measurement is inconsistent with a bounding box containing the beacon measurements. An original formulation using sum-of-squares and linear matrix inequalities is derived toBy test if a a measurement measurement isfor inconsistent with boundingitbox box containing the beacon derived test if inconsistent aa bounding containing beacon position. performing this test each range measurement, is possible to recursively position.to By performing this test test is for each range rangewith measurement, itbox is possible possible to the recursively derived toBy test if a measurement isfor inconsistent with a boundingit containing the beacon position. performing this each measurement, is to recursively position. By performing this test for each range measurement, it is possible to recursively eliminate incompatible boxes and find the smallest consistent box. The box search is done eliminate By incompatible boxes and for findeach the smallest smallest consistent box. box. The box search search is done done position. performingboxes this test range measurement, it isThe possible to recursively eliminate incompatible and find the consistent box is eliminate incompatible boxes and find the smallest consistent box. The box search is done with a breadth-first search algorithm that recursively prunes inconsistent boxes and splits the with a breadth-first search algorithm that recursively prunes inconsistent boxes and splits the eliminate incompatible boxes and find the smallest consistent box. The box search is done with a to breadth-first search algorithm that recursively prunes inconsistent boxes and splits splits and the with a breadth-first search algorithm that recursively prunes inconsistent boxes and the others narrow the estimation. The validity of the method is asserted via simulations others to narrow the estimation. The validity of the method is asserted via simulations and with a breadth-first search algorithm that recursively prunes inconsistent boxes and splits the others to narrow narrow the estimation. Themethods. validity of of the method method is asserted via simulations and others to estimation. The validity the asserted simulations compared to other standard mapping Different levels and types of noise are added to compared to other otherthe standard mapping methods. Different levels is and types of ofvia noise are added addedand to others to narrow the estimation. Themethods. validity of the method is asserted via simulations and compared to standard mapping Different levels and types noise are to compared to other standard mapping methods. Different levels and types of noise are added to evaluate the performances of the algorithm. It resulted that the approach accommodates very evaluate the performances of the algorithm. It resulted that the approach accommodates very compared to other standard mapping methods. Different levels and types of noise are added to evaluate the of algorithm. It resulted that approach accommodates very evaluate the performances performances of the the algorithm. It by resulted that the the accommodates well classical zero-mean white Gaussian noises adaptating the ratio of tolerated outliers for well classical classical zero-mean white white Gaussian noises by adaptating theapproach ratio of of tolerated tolerated outliersvery for evaluate the performances of the algorithm. It by resulted that the approach accommodates very well zero-mean Gaussian noises adaptating the ratio outliers for well classical zero-mean white Gaussian noises by adaptating the ratio of tolerated outliers for the consistency check, but fails to handle additive biases. the consistency check, but fails to handle additive biases. well classical zero-mean white Gaussian noises by adaptating the ratio of tolerated outliers for the consistency consistency check, check, but but fails fails to to handle handle additive additive biases. biases. the the consistency check, butFederation fails to handle additive biases. © 2017, IFAC (International of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Mapping, Range-Only, Beacon, LMI, SoS Keywords: Mapping, Range-Only, Beacon, LMI, SoS Keywords: Keywords: Mapping, Mapping, Range-Only, Range-Only, Beacon, Beacon, LMI, LMI, SoS SoS Keywords: Mapping, Range-Only, Beacon, LMI, SoS 1. INTRODUCTION the robot positions are known without errors. However, our 1. INTRODUCTION INTRODUCTION the robot robot positions positions are are known known without without errors. errors. However, However, our our 1. the 1. INTRODUCTION the robot positions are known without errors. However, our approach could also be used for localization and possibly approach could also be used for localization and possibly 1. INTRODUCTION the robot positions are known without errors. However, our approach couldproblem. also be be used used for for localization localization and and possibly possibly approach could also for RO-SLAM for RO-SLAM RO-SLAM problem. approach couldproblem. also be used for localization and possibly In robotics, the mapping problem consists in constructing for In robotics, the mapping problem consists in constructing for RO-SLAM problem. In robotics, the the mapping mapping problem consists consists in constructing constructing RO-SLAM In robotics, problem in a representation of the environment. The latter can then Regarding the mapping problem, popular technique a representation representation of the the environment. environment. The latter latter can then then for Regarding the problem. mapping problem, problem, aaa popular popular technique technique In robotics, the mapping problem consists in constructing of The can Regarding the mapping aabe representation of the environment. The latter can then Regarding the mapping problem, a popular technique used by a robot to plan its trajectory, navigate and introduced by Moravec and Elfes (1985) is the occupancy be used by a robot to plan its trajectory, navigate and introduced by Moravec and Elfes (1985) is the occupancy a representation of the environment. The latter can then Regarding the mapping problem, a popular technique be used by a robot to plan its trajectory, navigate and introduced byItMoravec Moravec and Elfes Elfes (1985) (1985) is the the occupancy occupancy be used by a robot tothere plan is navigate and by and is avoid obstacles. When no a priori information on grid method. uses probabilistic framework to update avoid obstacles. When there isits notrajectory, priori information information on introduced grid method. method. ItMoravec uses aaa probabilistic probabilistic framework to update update be used by a robot tothere plan is itsno trajectory, navigate and introduced byIt and Elfes (1985) is the occupancy avoid obstacles. When aatopriori priori on grid uses framework to avoid obstacles. When there is no a information on grid method. It uses a probabilistic framework to update the robot positions, this is referred as the simultaneous the cell occupancy probability, and provides a 2D or 3D the robot positions, this is referred to as the simultaneous the cell occupancy probability, and provides a 2D or 3D 3D avoid obstacles. When there is no a priori information on grid method. It uses a probabilistic framework to update the robot positions, positions, this is is(SLAM). referred to to as topic the simultaneous simultaneous the cell occupancy occupancy probability, andenvironment. provides aa 2D or the robot this referred as the the cell probability, and provides 2D or 3D localization and mapping This has already discretized representation of the In order localization and mapping (SLAM). This topic has already discretized representation of the environment. In order the robot positions, this is referred to as the simultaneous the cell occupancy probability, and provides a 2D or 3D localization and mapping mapping (SLAM). This topic topic has already discretized representation ofvehicle, the environment. environment. In (2006) order localization and (SLAM). This already the order been extensively covered in the literature even for the less to localize an underwater Olson et al. been extensively extensively covered in in the literature literature even has for the the less discretized to localize localize representation an underwater underwater of vehicle, Olson et et al. al.In (2006) localization and mapping (SLAM). This topic has already discretized representation ofvehicle, the environment. In (2006) order been covered the even for less to an Olson been extensively covered in the literature even for the less to localize an underwater vehicle, Olson et al. (2006) common range-only (RO) case, where range measurements use a grid representation and a voting scheme strategy common range-only (RO)in case, where range measurements use localize a grid grid representation representation and a voting voting scheme strategy been extensively covered the where literature even for the less to an underwaterand vehicle, Olson et al.strategy (2006) common range-only (RO) case, range measurements use a a scheme common range-only (RO) case, where range measurements a grid aarepresentation a voting scheme strategy are the only exteroceptive information available, as it is to obtain first estimate of the beacon positions. By are the the only only exteroceptive information available, as it it is is use to obtain obtain first estimate estimateand of the the beacon positions. By common range-only (RO) case, where range measurements use a grid arepresentation and a voting scheme strategy are exteroceptive information available, as to first of beacon positions. By are the only exteroceptive information available, as it is to obtain a first estimate of the beacon positions. By the case in this work. For instance, Djugash and Singh computing the intersection of two range measurements, the case in this work. For instance, Djugash and Singh computing the intersection of two range measurements, are the only exteroceptive information available, as it is to obtain a first estimate of the beacon positions. By the case inanthis this work. For For instance, Djugash anda Singh Singh computing the intersection intersection of twoforrange range measurements, the case work. instance, and the two measurements, (2009) use extended Kalman filter (EKF) with polar the two solutions obtained vote a cell in the grid. (2009) usein anthis extended Kalman filter Djugash (EKF) with with polar computing the two two solutions solutions obtained of vote forrange cell in the the grid. grid. the case inan work. For instance, Djugash andaa Singh computing the intersection of twofor measurements, (2009) use extended Kalman filter (EKF) polar the obtained vote aa cell cell in (2009) use an extended Kalman filter (EKF) with a polar the two solutions obtained vote for a in the grid. representation of the beacon positions. Later on, Caballero Ultimately, the cell with the largest support is considered representation of the beacon positions. Later on, Caballero Ultimately, the cell with the largest support is considered (2009) use an extended Kalman filter (EKF) with a polar the two solutions obtained vote for a cell in the grid. representation of the the beacon beacon positions. Later on,aCaballero Caballero Ultimately, the cell cell with the the largest This support is considered considered representation of Later on, the with largest support is et al. (2010) improved the formulation with mixture as the estimated beacon positions. estimate is then et al. al. (2010) (2010) improved improved the positions. formulation with mixture Ultimately, as the the estimated estimated beacon positions. This estimate is then then representation of the beacon positions. Later on,aaCaballero Ultimately, the cell with the largest This support is considered et the formulation with mixture as beacon positions. estimate is et al. (2010) improved the formulation with a mixture as the estimated beacon positions. This estimate is then of Gaussians to obtain an undelayed initialization of the used as an initialization in an EKF localization algorithm. of Gaussians to obtain an undelayed initialization of the used as an initialization in an EKF localization algorithm. et al. (2010) improved the formulation with a mixture as the estimated beacon positions. This estimate is then of Gaussians tofilter. obtain an formulation undelayed initialization initialization of the the used used as an antechnique initialization incan an be EKF localization algorithm. of Gaussians obtain an undelayed of as initialization an EKF localization algorithm. beacons in the The was then extended Another that used for mapping is the beacons in the theto filter. The formulation was then then extended extended Another technique that in can be used for mapping mapping is the the of Gaussians tofilter. obtain an formulation undelayed initialization of the Another used as antechnique initialization incan an be EKF localization algorithm. beacons in The was that used for is beacons in the filter. The formulation was then extended Another technique that can be used for mapping is the to the 3D case with a spherical representation (Fabresse trilateration algorithm (Zhou, 2009). The principle is to to the 3D case with a spherical representation (Fabresse trilateration algorithm (Zhou, 2009). The principle is to beacons in the filter. The formulation was then extended Another technique that can be used for mapping is the to the 3D caseBlanco with aaetspherical spherical representation (Fabresse trilateration algorithm (Zhou, 2009). The The principle principle isand to to the 3D case with representation (Fabresse trilateration algorithm (Zhou, 2009). is to et al., 2013). al. (2008) follow the same idea combine at least three range measurements to form et al., 2013). Blanco et al. (2008) follow the same idea combine at least three range measurements to form and to the 3D case with a spherical representation (Fabresse trilateration algorithm (Zhou, 2009). The principle is to et al., 2013). Blanco et framework al. (2008) (2008) follow follow the same idea combine at least least three threeproblem. range measurements measurements to form and et 2013). Blanco et al. at range but using a particle filter instead. In Boots and solve least-squares The solution can then be butal., using particle filter framework instead.the In same Boots idea and combine solve aaa least-squares least-squares problem. The solution solution to canform thenand be et al., 2013). Blanco et framework al. (2008) follow the same idea combine at least threeproblem. range measurements to form and but using aa particle particle filter instead. In Boots and solve The can then be but using a filter framework instead. In Boots and solve a least-squares problem. The solution can then be Gordon (2012), a spectral learning approach is proposed refined by any nonlinear optimization algorithm. The main Gordon (2012), a spectral learning approach is proposed refined by any nonlinear optimization algorithm. The main but using(2012), a particle filter framework instead. InisBoots and refined solve a by least-squares problem. The solution canThe then be Gordon a spectral learning approach proposed any nonlinear optimization algorithm. main Gordon (2012), a spectral is Di proposed by any nonlinear algorithm. The main to solve the problem. More related to our work, Marco drawback of most of the presented methods is their lack to solve solve the the problem. More learning related to toapproach our work, work, Di Marco refined drawback of most most of the theoptimization presented methods methods is their their lack Gordon (2012), a spectral learning approach is Di proposed refined by any nonlinear optimization algorithm. The main to problem. More related our Marco drawback of of presented is lack to solve the problem. More related to our work, Di Marco drawback of most of the presented methods is their lack et al. (2004) use a set theoretic approach and unknownof robustness and sensitivity to outliers. As we will see et al. (2004) use a set theoretic approach and unknownof robustness and sensitivity to outliers. As we will see to solve the problem. More related to our work, Di Marco drawback of most of the presented methods is their lack et al. (2004) (2004) use use set theoretic approach andguaranteed unknown- of of robustness and can sensitivity to outliers. outliers. As As we will see see et al. aa set theoretic approach and unknownrobustness and sensitivity to we will but-bounded errors to recursively estimate later, our method nicely accomodate the outliers by but-bounded errors to recursively estimate guaranteed later, our method can nicely accomodate the outliers by et al. (2004) use a set theoretic approach and unknownof robustness and sensitivity to outliers. As we will see but-bounded errors to robot recursively estimate guaranteed later, our amethod method can nicely nicely accomodate the outliers by but-bounded errors to recursively estimate guaranteed later, our can accomodate the outliers by uncertain regions of the and beacon positions. In the adjusting parameter. Compared to most of the previous uncertain regions regions of the the robot and beacon beacon positions. In the the adjusting adjusting parameter. Compared to most most the of the the previous but-bounded errors to robot recursively estimate guaranteed later, our aamethod can nicely accomodate outliers by uncertain of and positions. In parameter. Compared to of previous uncertain regions of the(2011, robot 2009) and beacon positions. In the adjusting Compared to most of theoffline previous same context, Jaulin uses interval analysis works, the proposed method works currently by same context, context, Jaulin (2011, 2009) uses interval interval analysis works, the theaa parameter. proposed method method works currently offline by uncertain regions of the(2011, robot 2009) and beacon positions. In the adjusting parameter. Compared to most of theoffline previous same Jaulin uses analysis works, proposed works currently by same context, Jaulin (2011, 2009) uses interval analysis works, the proposed method works currently offline by and constraint satisfaction methods to propagate and first collecting all the range measurements along the robot and constraint satisfaction methods to propagate and first collecting all the range measurements along the robot same context, Jaulin (2011, 2009) uses interval analysis works, the proposed method works currently offline by and constraint satisfaction methods to propagate propagate and first first collecting all the processing range measurements measurements along the robot and constraint satisfaction methods to and collecting the range robot reduce the intervals representing the estimated variables. trajectory and then them. But this allows to reduce the intervals intervals representing the estimated estimated variables. trajectory andall then processing them. But Butalong this the allows to and constraint satisfaction methods to propagate and first collecting all the processing range measurements along the robot reduce the representing the variables. trajectory and then them. this allows to reduce the intervals representing the estimated variables. trajectory and then processing them. But this allows to adjust the number of tolerated outliers in the search adjust the number of tolerated outliers in the search reduce the intervals representing the estimated variables. trajectory and then processing them. But this allows to In this work, an interval representation of the estimated adjust the number of estimated tolerated beacon outlierspositions. in the the search search In this this work, work, an an interval interval representation representation of of the the estimated estimated adjust number of tolerated outliers in process and to refine the Thus, In process the and to to refine the the estimated beacon positions. Thus, adjust the number of estimated tolerated beacon outlierspositions. in the search In this work, anadopted. interval representation of the estimated variables is also However, contrary to the previprocess and refine Thus, variables is also adopted. However, contrary to the previprocess and to refine the estimated beacon positions. Thus, the proposed method is intended for retrieving the position In this work, anadopted. interval representation of the estimated variables is also However, contrary to the previthe proposed method is intended for retrieving the position process and to refine the estimated beacon positions. Thus, variables is also adopted. However, contrary to the previously cited works, here only the pure mapping problem is the proposed method is beacons intended in for aretrieving retrieving the position position ously cited cited works, here only only the pure pure mapping problem is the proposed intended for the of indoor or outdoor garantueed way by variables is works, also adopted. However, contrary to problem the previously here the mapping is of indoor indoor or method outdooris beacons in garantueed way by by the proposed method is beacons intended in for aaretrieving the position ously cited works, here only we the purethe mapping problem is considered. This means that make assumptions that of or outdoor garantueed way considered. This means that we make the assumptions that of indoor or outdoor beacons in a garantueed way by ously cited works, here only the pure mapping problem is considered. This This means means that that we we make make the the assumptions assumptions that that of indoor or outdoor beacons in a garantueed way by considered. considered. This means that we make the assumptions that

Lionel Lionel Lionel Lionel Lionel

Copyright © 2017, 2017 IFAC 11983Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright 2017 IFAC 11983 Copyright © 2017 IFAC 11983 Copyright ©under 2017 responsibility IFAC 11983 Peer review of International Federation of Automatic Control. Copyright © 2017 IFAC 11983 10.1016/j.ifacol.2017.08.1598

Proceedings of the 20th IFAC World Congress 11492 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497 Toulouse, France, July 9-14, 2017

returning an interval enclosing the true position of the beacons when the measurements are corrupted by many outliers. In this work, the mapping problem with known robot positions is treated with an original formulation combining linear matrix inequalities (LMI) (Nesterov and Nemirovski, 1994) and sum-of-squares (SoS) polynomials (M.D. Choi, 1995; Lasserre, 2007). LMI (Gahinet et al., 1995) are extensively used in control engineering and became a powerful tool in many domains. Moreover, thanks to the works of Nesterov and Nemirovski (1994) on the interior point method, LMI can now be efficiently solved. The method presented in this paper is inspired by the work developed by Pani Paudel et al. (2015), initially desgined for point-to-plane registration, and adapted here for the mapping problem. Using only range measurements between a mobile robot with known positions and several unknown static beacons scattered in the environment, the objective is to recover the location of the beacons. This is done by checking that all the corresponding range measurements are compatible with a bounding box supposed to contain the beacon position. The consistency test is done by transforming the range measurement constraints into a polynomial inequality. If this polynomial is SoS, the corresponding measurement is inconsistent. Checking that a polynomial is SoS is then efficiently solved as a LMI feasability problem. Notice that this technique does not require a noise model or hypotheses for the measurements, and that it is possible to make it robust with respect to erroneous measurements occuring in real cases by controlling the percentage of measurements that do not satisfy the consistency test. The rest of this paper is organized as follows. Section II introduces the problem and the formulation used throughout the paper. Section III describes the algorithm and explains in detail the consistency check procedure. Section IV presents the various simulation results obtained with increasing noise levels. Finally Section V concludes the paper and gives some future perspectives. 2. PRELIMINARIES This section briefly introduces the concepts used by the proposed approach and then describes how they are integrated in the formulation. 2.1 Technical background Set-membership functions: For a given interval [x, x], let us consider the membership function g(x) such that: g(x) = (x − x)(x − x) (1) The function is positive inside the interval and negative outside. Thus, x ∈ [x, x] iff g(x) ≥ 0. A 2D box is defined as the product of two intervals:    B = x = (x, y) ∈ R2  (x, y) ∈ [x, x] × [y, y]

2.2 Application to the approach For the mapping problem, the trajectory of the robot is known and represented by a set of K robot positions: xrk = (xrk , yrk )T , k ∈ [0, K] Note that the orientation information is not used here. The objective is to find the position of the N static beacons defined by: xbi = (xbi , ybi )T , i ∈ [1, N ] The proposed method is based on the ability to determine if a given box does not enclose the true position of the beacon. Thus, using the notation of (1) and (2), a box is represented by:    Bi := xbi ∈ R2  gx (xbi ) ≥ 0, gy (xbi ) ≥ 0 with:



gx (xbi ) = (xbi − xbi )(xbi − xbi ) gy (xbi ) = (ybi − ybi )(ybi − ybi )

The range measurements between the robot and the beacons are the only information available to recover the beacon positions xbi . We define the constraint fk,i (xbi ) as the difference between the squared predicted range d2k,i 2 and the squared measured range rk,i between the robot pose at time k and the beacon i: 2 − d2k,i (3) fk,i (xbi ) = rk,i

with d2k,i = (xbi − xrk )2 + (ybi − yrk )2 . A measurement is consistent if fk,i crosses zero at some point inside the box representing the beacon position. It should be noticed that the constraint (3) is quadratic with respect to the unknown xbi , which makes it interesting for a SoS approach. In the next section, this constraint will be exploited to develop a method retrieving a beacon box consistent with all or most of the range measurements, by testing its positivity over a given box. 3. ALGORITHM DESCRIPTION

(2)

Sum-of-Squares (SoS) and Linear matrix inequalities (LMI): A scalar polynomial function p(x) is SoS if there exists a polynomial q(x) such that:  p(x) = qi (x)qi (x) i

A polynomial function p(x) is positive if it is SoS. This notation can be easily extended to the case of matrix inequalities. Moreover, if we have: P(x, l) = QT (x)M(l)Q(x) with M(l) a symmetric matrix function of l then P(x, l) is positive if M(l) ≥ 0 1 and P(x, l) is said to be SoS. In the case where M(l) is affine in l, M(l) ≥ 0 is called a linear matrix inequality. Checking the positivity of a polynomial function P(x, l) can thus be solved via a LMI. Finally, the positivity can be restricted to a domain. For instance in 2D: P(x) is positive over B if there exists positive scalars σx and σy such that: P(x) − σx gx (x) − σy gy (x) is SoS, where gx (x) and gy (x) are the membership functions for the x and y components of x.

In this section, we first give an outline of the algorithm. Then, we delve into the derivation of the LMI consistency test. Finally, the procedure to explore and search the smallest compatible box is presented. 1 For a real symmetric matrix, M ≥ 0 means that all the eigenvalues of M are non-negatives and reals.

11984

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497

3.1 Algorithm outline The proposed method attempts to find, for each beacon, the smallest box where all the range measurements (or a predefined percentage of them) respect the condition: 2 − d2k,i = 0 fk,i (xbi ) = rk,i

(4)

More precisely, we count the number of range measurements satisfying the condition (4) for some point inside that box (notice that these points may not be the same for all range measurements due to noise). The condition is verified by testing the positivity of the constraint (3) and by solving a LMI feasibility problem. If it is feasible, then the constraint (3) is SoS and the condition (4) is not respected. Thus the range measurement is an outlier. By controlling the number of outliers, it is possible to discard or accept the considered box. The accepted boxes are then split and the test is repeated for each of the new boxes. This process is iterated until the algorithm has discarded all the potential boxes and a smaller consistent box cannot be found. The consistency test performed for a given box and a range measurement is now detailed.

3.2 LMI consistency check We define the polynomial function hk,i (xbi ) constructed from the constraint (3) and the membership quadratic polynomial (1):  gj (xbi )σj (5) hk,i (xbi ) = λfk,i (xbi ) − j={x,y}

where λ ∈ R and σj ∈ R . The variable λ is added to take into account the cases: ±fk,i > 0. The equation (5) can be rewritten in a quadratic form as: +

hk,i (xbi ) = QT (xbi )G(λ, σx , σy )Q(xbi )

(6)

where G(λ, σx , σy ) is the Gram matrix (Powers and Wormann, 1998) of hk,i (xbi ) and QT (xbi ) = (xbi , ybi , 1). The method stated in Pani Paudel et al. (2015) is adapted: for a given box [xbi , xbi ], if there exists a real scalar λ and positive scalars σj , j = {x, y}, such that the polynomial hk,i (xbi ) is SoS, then λfk,i (xbi ) > 0 for every xbi ∈ [xbi , xbi ]. In this case, fk,i is either strictly positive, either strictly negative 2 over the whole domain and thus contains no point inside the domain for which fk,i = 0. The range measurement rk,i is then guaranteed to be an outlier within the considered bounds. Otherwise, rk,i is a potential inlier. Verifying that hk,i (xbi ) is SoS can be done by checking that its corresponding real symmetric Gram matrix from (6) is positive semi-definite: G(λ, σx , σy ) ≥ 0 (Powers and Wormann, 1998; M.D. Choi, 1995). The latter condition can be verified by solving a LMI feasability problem for the variables λ and σj . Indeed, the matrix G(λ, σx , σy ) can be decomposed as: G(λ, σx , σy ) = λG1 + σx G2 + σy G3 with: 2

In fact only if xbi ∈ ]xbi , xbi [

(7)

11493

 1 0 −xrk  1 −yrk (8) G1 =  0 2 −xrk −yrk x2rk + yr2k − rk,i   1 1 0 − (xbi + xbi )   2  0 0 0 (9) G2 =    1 xb i x b i − (xbi + xbi ) 0   2 0 0 0 1 0 1 − (ybi + ybi )  (10) G3 =  2   1 yb i y b i 0 − (ybi + ybi ) 2 To complete the LMI problem, the positivity constraints on the σj must be added: σx > 0 and σy > 0. 

In summary, if the LMI problem is feasible, then we have: G(λ, σx , σy ) ≥ 0, thus hk,i (xbi ) is SoS, meaning that the range measurement is an outlier. Otherwise, if the LMI solver didn’t find any solution, the range measurement is a potential inlier. Now that it is possible to test if a range measurement is consistent with a given box, it remains to define a procedure to combine all the constraints and search for the smallest compatible box. The next paragraph presents this procedure based on a breadth-first search (BFS) algorithm that was used to find the boxes. 3.3 Complete algorithm A breadth-first search algorithm (Lee, 1961) was developed to find the smallest box consistent with all the range measurements with respect to one beacon. Thus, for N beacons, the search has to be executed N times. The algorithm takes as input the robot positions, the range measurements associated with the beacon, an initial box and the thresholds to control the percentage of outliers and stop the search when a predefined accuracy is obtained. Starting with a large initial box, the algorithm recursively divides the boxes until they are not consistent anymore with the measurements. The consistency test is carried out for each range measurement, and if an outlier is detected, or if the percentage of tolerated outliers reaches a predefined threshold, then the box is discarded. The ability to control the percentage of outliers is very important when we deal with real measurements corrupted by noise, as it will be shown in the experiments. To determine which box has to be explored, a list of consistent boxes is maintained. If a box passes the test, it is added to the list, otherwise it is discarded. The search stops when the list is empty or when a given accuracy is reached. The accuracy threshold is defined with a minimal box area set to 0.01 m2 during the simulations. One critical issue encountered during the simulations was the process of splitting a box into smaller boxes. When the true beacon position is near one of the inside borders created during the division process, the consistency check can fail and stop the search prematurely, resulting in a raw evaluation of the beacon position. It is even more problematic when noise is present. In the proposed algorithm, this problem is handled by using a two pass division. The interval is first split in four, and if none of the sub-boxes passes the test, another division scheme is used. This way, we were able to increase the robustness of the algorithm, but at the

11985

Proceedings of the 20th IFAC World Congress 11494 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497 Toulouse, France, July 9-14, 2017

cost of longer search. Other strategies are possible, such as using jointly a local method, and will be considered in future work to improve the process. In the next section, the experiments conducted to validate the approach are presented.

Table 4. Scenario 4 results Noise No noise, 0% outliers Noise 1, 0% outliers Noise 1, 5% outliers Noise 1, 10% outliers Noise 2, 0% outliers Noise 2, 5% outliers Noise 2, 10% outliers

4. SIMULATION RESULTS

Table 1. Scenario 1 results Noise No noise, 0% outliers No noise, 5% outliers

B1 area [m2 ] 0.171 0.171

B2 area [m2 ] 0.012 0.012

B3 area [m2 ] 0.049 0.049

cputime [s] 8.4 20.5

Table 2. Scenario 2 results Noise No noise, 0% outliers No noise, 5% outliers

B1 area [m2 ] 0.014 0.014

B2 area [m2 ] 0.014 0.014

B3 area [m2 ] 0.027 0.027

B4 area [m2 ] 0.027 0.027

cputime [s] 23.8 44.5

Table 3. Scenario 3 results Noise No noise, 0% outliers Noise 1, 0% outliers Noise 1, 5% outliers Noise 1, 10% outliers Noise 1, 20% outliers

B1 area [m2 ] 0.018 2.34 0.29 0.15 0.15

B2 area [m2 ] 0.018 0.58 0.19 0.29 0.58

B3 area [m2 ] 0.018 4.69 0.15 0.15 0.15

B4 area [m2 ] 0.018 1.56 0.29 0.29 0.44

B5 area [m2 ] 0.037 2.08 4.69 2.34 0.58

B6 area [m2 ] 0.018 9.37 0.15 0.15 0.15

cputime [s] 816.4 433.8 649.9 743.2 990.9

B2 area [m2 ] 0.015 126.56 31.64 31.64 63.28 15.82 15.82

B3 area [m2 ] 00.015 126.56 63.28 63.28 253.12 42.19 10.55

B4 area [m2 ] 0.046 42.19 42.19 126.56 506.25 31.64 47.46

cputime [s] 896.5 338.7 371.6 448.9 224.8 321.2 390.0

4.1 Mapping results without noise The robot trajectories (black line), true beacon positions (red dots) and corresponding returned boxes (yellow or magenta rectangles) are shown in Fig. 2 and Fig. 3 for Scenarios 1 and 2 respectively. The returned boxes are not visible because they are too small (see the corresponding areas in Table 1) and are hidden by the red dots of the true beacon positions. From Fig. 1 and beacon B1, one notices the ring shape of the potential boxes that were explored. This is due to the small size of the trajectory and small number of range measurements. Moreover in the same figure, but for B2, one sees that the search was principaly concentrated on two symmetric positions which is commom for range meaurements because of the multi-modality of the constraints. By comparing the two lines of Table 1 and Table 2, we can notice that increasing the percentage of tolerated outliers from 0% to 5% does not reduce the size of the returned boxes. In the next section, we will see that when additional noise corrupts the measurements, a reduction is observed when the percentage is increased. Beacon 1 box search

Beacon 2 box search

10

10

8

8

B1

6

6

4

4

2

2

Y [m]

Y [m]

In order to evaluate the proposed mapping algorithm, simulations of different kinds were conducted. Four different datasets, were employed. The first two (Scenarios 1 and 2) were created to test the method for simple cases and without noise in order to validate the approach. They contain respectively 15 and 160 range measurements. The third dataset comes from a ROS/Gazebo simulation (Scenario 3), whereas the fourth is a publicly available dataset (Scenario 4) presented by Djugash et al. (2009). These last two datasets were used to validate that the proposed algorithm can also perform well in more challenging conditions with different levels of noise. The ROS/Gazebo dataset contains 6654 range measurements and was used with and without noise. The noise was generated following a zero-mean white Gaussian distribution with a standard deviation of 0.1 m. The scenario 4 dataset contains 3434 range measurements, and was used in three different configurations: i/ without noise, ii/ with noise and no bias, and iii/ with the original range measurements from the dataset. The values for the bias are between −2.6 and −2.9 m, whereas the standard deviations are between 1 and 1.2 m. The different noises are referred in the rest of this section as: Noise 1 when there is only a white Gaussian noise, and Noise 2, when there is a white Gaussian noise and additional bias. In order to evaluate the noise accomodation, different percentage (0%, 5%, 10% and 20%) of tolerated outliers were tested during the simulations. The algorithm and simulations are written in Matlab with the LMI Toolbox to solve the LMI feasability problem (7). Tables 1 to 4 report the area of the beacon intervals found by the algorithm, for the four scenarios respectively. In the following, the simulation results are presented with increasing level of noise. The mapping results are presented in the following order: without noise, with additional white Gaussian noise, with bias, and finally with outliers.

B1 area [m2 ] 0.015 31.64 31.64 15.82 42.19 253.12 15.82

0 −2

−2

−4

−4

−6

−6

−8

−8

−10

−10 −10

−5

0

X [m]

5

10

B2

0

−10

−5

0

X [m]

5

10

Fig. 1. Scenario 1 (No noise): beacon boxes 4.2 Mapping results with zero-mean white Gaussian noise Scenarios 3 and 4 were used to test the algorithm against range measurements with zero-mean white Gaussian noise. The results are presented on the rows corresponding to Noise 1 in Tables 3 and 4. By looking at the final box areas (around a few meters square to hundred meters square) for the case of Noise 1 with 0% of outliers tolerated and by considering Fig. 4 and Fig. 5, it seems that the algorithm performs poorly in presence of noise. But in these simulations, no outiers were tolerated. Indeed, by looking at Fig. 6 for Scenario 3 and Fig. 7, Fig. 8 for Scenario 4 where 10% of outliers were tolerated, the returned boxes are smaller, and still contains the true solution. When no outliers are tolerated, the box sizes are larger and even may not contain the true solution (Fig. 4, 11986

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497

Robot trajectory and final beacon boxes

B1 box

B3,r=0,a=1.526e−3

2

B2,r=0, a=1.526e−3

0

0 −10

−10 −10 0 10

X (m) B4 box

−6

−10

−5

0

X (m)

5

Y (m)

−10 10

−10

15

B1, r=0, a=1.72e−3

10 6

B2, r=0, a=1.72e−3

0

5

X (m)

10

−10 0 10

X (m)

X (m)

B2, r=0, a=0.585

B4, r=0, a=9.375

4

Y (m) −5

−10 −10 0 10

B1, r=0, a=2.344 B6, r=0, a=9.375

8

−5

−10 −10

0

Robot trajectory and final beacon boxes

10

B4, r=0, a=1.72e−3

B5 −10

X (m)

B3, r=0, a=1.72e−3

0

0

B6

Fig. 4. Scenario 3 (Noise 1, 0% outliers tolerated): beacon boxes (in yellow)

Robot trajectory and final beacon boxes

5

10

−10 0 10

Fig. 2. Scenario 1 (No noise): final (r=% of outliers, a=box area)

X (m) B6 box

10

B44 0 Beacon

B3 −10 0 10

X (m) B5 box

10

−8

0 −10

−10 0 10

−2 −4

Y (m)

B2

0

Y (m)

Y (m)

4

B1

B3 box 10

Y (m)

6

Y (m)

B1,r=0,a=1.526e−3

8

B2 box 10

Y (m)

10

Y (m)

10

11495

15

B3, r=0, a=4.687

2 0 −2 −4

B5, r=0, a=150

−6 −8

Fig. 3. Scenario 2 (No noise): final (r=% of outliers, a=box area) B4). This result shows that the noise has a significant impact on the box division procedure. Even when the box is still very large, if one of its border is close to the true beacon position, with only one noisy range measurement considered as an outlier, the search can deviated from the true beacon position. This influence is attenuated when some outliers are tolerated. 4.3 Mapping results with bias It can be seen in the results given in Table 4 and Fig. 9 obtained from Scenario 4 that a bias on the range measurements impacts the size of the returned boxes. As for the case with Gaussian noise, increasing the percentage of tolerated outliers helps to reduce the area of the boxes. However, it can be seen from Fig. 9 that the results of the proposed method are corrupted by a bias, because all the returned intervals are slightly shifted from the true beacon positions.

−10 −15

−10

−5

0

X (m)

5

10

15

Fig. 5. Scenario 3 (Noise 1, 0% outliers tolerated): final (r=% of outliers, a=box area) 4.4 Mapping results with outliers In order to evaluate the performance of our method in presence of outliers, we conducted a simulation with Scenario 4 with an increasing percentage of outliers (from 0% to 40%) for each beacon. Moreover, we also compared the results with three standard mapping algorithms. The first one is a trilateration based algorithm (termed Trilat). The second, is a Levenberg-Marquardt nonlinear optimization algorithm (termed LM) which is initialized from the trilateration solution. The third is a version of an occupancy grid algorithm (termed OccGrid). As shown in Fig. 10 for beacon 1, we computed the estimated beacon errors for an increasing percentage of outliers. We can see that both our method (LMI-SoS) and the OccGrid algorithm are

11987

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 11496 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497

Robot trajectory and final beacon boxes 10

8

70

B2, r=0.065, a=0.293

B1, r=0.061, a=0.146 B6, r=0.048, a=0.146

50

B4,

2

B5, r=0.078, a=2.344

0

B3, r=0.026, r=0.046, a=0.146 a=0.293

30 20 10 0

B1, r=0.083, a=15.82

−4

−10

−6 −15

−20 −60

−10

−5

0

X (m)

5

10

15

Fig. 6. Scenario 3 (Noise 1, 10% outliers tolerated): final (r=% of outliers, a=box area)

B1 box B1

0 −20 −60 −40 −20

0

X (m) B3 box

70

40

60

20 0

X (m) B4 box

60

B33 Beacon

20 0

−20 −60 −40 −20

0

X (m)

20

Y (m)

60 40

0

40

X (m)

0

20

B4, r=0.073, a=47.46

40

20

B3, r=0.081, a=10.54

30 20 10

Beacon 4 B4

0

20

−10

0 −20 −60 −40 −20

−20

50

B2 Beacon 2

−20 −60 −40 −20

20

−40

Robot trajectory and final beacon boxes

60

Y (m)

20

Y (m)

40

B2, r=0.007, a=31.64

Fig. 8. Scenario 4 (Noise 1, 10% outliers tolerated): final (r=% of outliers, a=box area)

B2 box

60

Y (m)

B3, r=0.023, a=63.28

40

4

−2

Y (m)

B4, r=0.061, a=126.56

60

Y (m)

Y (m)

6

Robot trajectory and final beacon boxes

0

X (m)

B1, r=0.099, a=15.82

−20 −60

20

Fig. 7. Scenario 4 (Noise 1, 10% outliers tolerated): beacon boxes (in yellow)

B2, r=0.032, a=15.82 −40

−20

X (m)

0

20

Fig. 9. Scenario 4 (Noise 2, 10% outliers tolerated): final (r=% of outliers, a=box area)

robust to the outliers. Indeed, even if the LM methods gives lower errors with no outliers, the errors quickly grow with the percentage of outliers. The Trilat algorithm shows the same behovior, whereas the OccGrid and the LMI-SoS keep the errors approximately constant. Fig. 11 shows the estimated beacon positions for the four algorithms and for 20% of outliers. The blue rectangles represent the final boxes returned by our LMI-SoS approach. We can see that the final boxes always include the true position, and that the estimated positions provided by OccGrid are also very close. As the Trilat algorithm estimates are far from the true positions, the LM algorithm fails to converge for all beacons. This shows the importance of a good initial estimate for nonlinear optimization. In this case, we could have used the positions estimated by our algorithm to initialize the LM algorithm and obtain a better estimate. 4.5 Discussion Thereby, with no noise, the boxes have very small areas which in most cases correspond to the threshold area

Fig. 10. Position errors of beacon 1 for increasing percentage of outliers

11988

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 Lionel Génevé et al. / IFAC PapersOnLine 50-1 (2017) 11491–11497

Fig. 11. Estimated beacon positions with 20% of outliers δarea where the algorithm is stopped. However, in presence of noise, the returned boxes are much larger. To handle correctly the noise, it is mandatory to tolerate a certain amount of outliers, otherwise the returned solutions can be totally incorrect. Because no noise model is used, the method does not seem suitable in the case of biases on the measurements. The main advantage of the proposed method, its robustness, was demonstrated with an increasing percentage of outliers. Compared to other standard methods, it was not affected by outliers and always returned a box enclosing the true beacon position. Finally by looking at the processing times in Tables 1 to 4, it appears that the method is relatively slow and only suited for offline applications. Indeed for each box and each range measurement, a LMI problem has to be solved. Indeed, the BFS algorithm used to find the smallest boxes is very costly and we plan to find an alternative in future works. 5. CONCLUSION This work presented a new approach for the mapping of beacons given known robot positions, and the range measurements as the only source of information. A LMI feasability test was derived to check if a range measurement is consistent with a box enclosing the true beacon. Using all the range measurements coming from a beacon, the proposed consistency test allows to discard regions of the search space incompatibles with one or a percentage of the measurements. Starting from an initially large box, and employing a breadth-first search algorithm, it is possible to return the smallest box consistent with all the constraints. The proposed algorithm was validated on different simulations with increasing levels of noises and showed promising performances. Indeed, the algorithm demonstrates its ability to handle noisy measurements and outliers by increasing a threshold on the percentage of tolerated outliers. By doing so, the size of the returned boxes where reduced and the estimation of the beacon positions was thus improved. One of the major asset of the proposed algorithm is its robustness to outliers. In future work, we plan to adapt the algorithm for the localization and SLAM problems, or to integrate it as an outlier removal module.

11497

IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS). Boots, B. and Gordon, G.J. (2012). A spectral learning approach to range-only slam. arXiv preprint arXiv:1207.2491. Caballero, F., Merino, L., and Ollero, A. (2010). A general gaussian-mixture approach for range-only mapping using multiple hypotheses. In IEEE Int. Conf. on Robotics and Automation (ICRA). Di Marco, M., Garulli, A., Giannitrapani, A., and Vicino, A. (2004). A set theoretic approach to dynamic robot localization and mapping. Autonomous Robots, 16(1), 23–47. Djugash, J., Hamner, B., and Roth, S. (2009). Navigating with ranging radios: Five data sets with ground truth. Journal of Field Robotics, 26(9), 689–695. Djugash, J. and Singh, S. (2009). A robust method of localization and mapping using only range. In Experimental Robotics, 341–351. Springer. Fabresse, F., Caballero, F., Maza, I., and Ollero, A. (2013). Undelayed 3d ro-slam based on gaussian-mixture and reduced spherical parametrization. In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS). Gahinet, P., Nemirovski, A., Laub, A.J., and Chilali, M. (1995). LMI Control Toolbox. The MathWorks Inc. Jaulin, L. (2009). A nonlinear set membership approach for the localization and map building of underwater robots. IEEE Trans. on Robotics, 25(1), 88–98. Jaulin, L. (2011). Range-only slam with occupancy maps: A set-membership approach. IEEE Trans. on Robotics, 27(5), 1004–1010. Lasserre, J.B. (2007). A sum of squares approximation of nonnegative polynomials. SIAM Review, 49(4), 651–669. Lee, C. (1961). An algorithm for path connections and its applications. IRE Trans. on Electronic Computers, EC-10(3), 346–365. M.D. Choi, T.Y. Lam, B.R. (1995). Sums of squares of real polynomials. 58, 103–126. Moravec, H. and Elfes, A. (1985). High resolution maps from wide angle sonar. In IEEE Int. Conf. on Robotics and Automation (ICRA). Nesterov, Y. and Nemirovski, A. (1994). Interior-point Polynomial Methods in Convex Programming. SIAM. Olson, E., Leonard, J.J., and Teller, S. (2006). Robust range-only beacon localization. IEEE Journal of Oceanic Engineering, 31(4), 949–958. Pani Paudel, D., Habed, A., Demonceaux, C., and Vasseur, P. (2015). Robust and optimal sum-of-squares-based point-to-plane registration of image sets and structured scenes. In IEEE Int. Conf. on Computer Vision (ICCV). Powers, V. and Wormann, T. (1998). An algorithm for sums of squares of real polynomials. Journal of Pure and Applied Algebra, 127(1), 99–104. Zhou, Y. (2009). An efficient least-squares trilateration algorithm for mobile robot localization. In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS).

REFERENCES Blanco, J.L., Fernandez-Madrigal, J.A., and Gonzalez, J. (2008). Efficient probabilistic Range-Only SLAM. In

11989