Safe and Smooth: Certified Continuous-Time Range-Only Localization

TL;DR

It is shown that the efficient local solver often finds the globally optimal solution (confirmed by the certificate), but it may converge to local solutions with high errors, which the proposed certificate correctly detects.

Abstract

A common approach to localize a mobile robot is by measuring distances to points of known positions, called anchors. Locating a device from distance measurements is typically posed as a non-convex optimization problem, stemming from the nonlinearity of the measurement model. Non-convex optimization problems may yield suboptimal solutions when local iterative solvers such as Gauss-Newton are employed. In this paper, we design an optimality certificate for continuous-time range-only localization. Our formulation allows for the integration of a motion prior, which ensures smoothness of the solution and is crucial for localizing from only a few distance measurements. The proposed certificate comes at little additional cost since it has the same complexity as the sparse local solver itself: linear in the number of positions. We show, both in simulation and on real-world datasets, that the efficient local solver often finds the globally optimal solution (confirmed by our certificate), but it may converge to local solutions with high errors, which our certificate correctly detects.

Figures (7)

• Figure 1: A flying drone measures distances to fixed anchor points in an arena. The two solutions are obtained by running a continuous-time range-only localization scheme from two different initializations. We propose an efficient optimality certificate based on Lagrangian duality that correctly identifies the left solution as the global minimum.
• Figure 2: Factor graph representation of the GP inference problem. Black factors represent the motion prior, blue are the range-only measurement factors.
• Figure 3: Sparsity patterns of the cost matrices obtained using zero-velocity or constant-velocity priors, for $N=3$ and $D=3$.
• Figure 4: Certificate value vs. rmse, using different motion priors, in simulations with $N=100$, $M=6$ and $D=2$. The group of solutions corresponding to the smallest cost out of 10 initializations are labelled optimal. Disregarding the highest noise levels, all optimal solutions are successfully identified (true positives, t.p.) and the certificate fails for suboptimal solutions (true negatives, t.n.). At the highest noise levels, a small proportion of optimal solutions are not certified (false negatives, f.n.), which is in line with the sufficiency of the certificate.
• Figure 5: Left: Computation time of our GN solver, evaluating the dual variables, and computing the certificate, respectively, with increasing number of positions $N$. Right: certificate evaluation on real data, comparing the final cost and rmse of solutions. Three solutions for three chosen datasets are highlighted in colors. For all datasets, the cost difference between certified solutions (cross) and uncertified solutions (circle) is small, but the resulting rmse difference is significant. Thanks to the proposed method, such suboptimal solutions can be avoided.