PaperlayerView exact revision on arXiv

Source-linked AI summary

SCORE: A Second-Order Conic Initialization for Range-Aided SLAM

Alan Papalia, Joseph Morales, Kevin J. Doherty, David M. Rosen, John J. Leonard

arXiv:2210.03177v1cs.RO

TL;DR

RA-SLAM combines range and relative-transformation measurements, but its non-convex estimation problem requires good initialization for quality solutions. SCORE relaxes the problem to an SOCP derived from a QCQP, then uses that solution to initialize the original problem. Simulated and real-world experiments report improved initialization results, including for challenging multi-robot cases.

  • Problem

    RA-SLAM combines range sensing with relative transformations, but its non-convex MAP problem is typically solved by local search, which guarantees only locally optimal solutions and makes good initialization important.

    Source: §I · p. 1

  • Method

    SCORE derives a convex SOCP relaxation from a QCQP formulation of RA-SLAM and uses the SOCP solution to initialize the original MAP problem.

    Source: §I · p. 1, B. Relaxing Equation (6) to a SOCP · p. 4

  • Results

    Simulated and real-world experiments show SCORE outperforms state-of-the-art initializations, with up to two orders of magnitude reduction in average pose error versus existing odometry-based initializations.

    Source: §VIII · p. 6, §IX · p. 7

  • Takeaways & Limitations

    SCORE provides quality initializations for challenging multi-robot RA-SLAM using only odometry and inter-robot range measurements.

    Source: §VIII · p. 6, §IX · p. 7

    Some scenarios still produce near-zero trivial solutions despite constraining the first rotation; these failures can be detected when estimated rotation determinants fall below 5e-2.

    Source: A. When Does SCORE Return Poor… · p. 7

Abstract

from arXiv · show

We present a novel initialization technique for the range-aided simultaneous localization and mapping (RA-SLAM) problem. In RA-SLAM we consider measurements of point-to-point distances in addition to measurements of rigid transformations to landmark or pose variables. Standard formulations of RA-SLAM approach the problem as non-convex optimization, which requires a good initialization to obtain quality results. The initialization technique proposed here relaxes the RA-SLAM problem to a convex problem which is then solved to determine an initialization for the original, non-convex problem. The relaxation is a second-order cone program (SOCP), which is derived from a quadratically constrained quadratic program (QCQP) formulation of the RA-SLAM problem. As a SOCP, the method is highly scalable. We name this relaxation Second-order COnic RElaxation for RA-SLAM (SCORE). To our knowledge, this work represents the first convex relaxation for RA-SLAM. We present real-world and simulated experiments which show SCORE initialization permits the efficient recovery of quality solutions for a variety of challenging single- and multi-robot RA-SLAM problems with thousands of poses and range measurements.

I. INTRODUCTION

RA-SLAM combines distance and relative-transformation measurements to estimate robot poses and landmarks, but its non-convex optimization makes good initialization important. SCORE uses a convex SOCP relaxation to initialize the original problem, which is then refined by nonlinear least squares.

I. INTRODUCTION

  • RA-SLAM combines range and relative rigid-transformation measurements to estimate robot poses and landmark positions, supporting applications in planetary, subterranean, and sub-sea environments –.Range measurements add substantial state-estimation difficulty, while further development could advance robot navigation.Source: §I · p. 1
  • Because robot orientations and range-measurement models make MAP estimation non-convex, standard local-search approaches guarantee only locally optimal solutions, making initialization important.The paper frames convex relaxation as an efficiently solvable global optimization problem used to approximate the original problem.Source: §I · p. 1
  • SCORE derives a convex SOCP relaxation from a QCQP formulation, uses its solution to initialize the original NLS problem, and refines that initialization with NLS.The paper presents the first QCQP formulation of RA-SLAM, connecting it to broader work –.Source: Figure 1 · p. 1, §I · p. 1
  • SCORE is designed for 2D and 3D problems with arbitrary numbers of robots and landmarks, and its SOCP form is compatible with existing convex optimization libraries and scales to large problems.The implementation is open-sourced.Source: §I · p. 1

II. RELATED WORK

Prior RA-SLAM initializations address subsets of the problem or rely on assumptions that limit their reach. Related pose-graph and sensor-network relaxations likewise differ in the measurements or variables they can accommodate.

A. Initialization in RA-SLAM

  • Single-robot spectral methods estimate beacons or jointly estimate poses and beacons, but do not account for sensor noise models or readily extend to multi-robot settings.Other multi-robot methods use coordinated movement to estimate a single relative transform, while relative-transform methods still rely on noisy odometric pose composition to initialize other poses.Source: A. Initialization in RA-SLAM · p. 2

B. Initialization and Convex Relaxation in State-Estimation

  • Pose-graph initializers use rotation averaging or sequential linear estimation, but omit range measurements and therefore do not extend to RA-SLAM.Convex relaxations for pose-graph SLAM and other estimation tasks generally do not extend to ranges; the cited range-based exception assumes known beacons and covers only single-robot localization.Source: B. Initialization and Convex… · p. 2
  • SCORE allows multi-robot RA-SLAM with rigid-transformation and range measurements without requiring a priori information, unlike the cited range-based convex-relaxation exception.The exception required known beacon locations and considered only single-robot localization using ranges.Source: B. Initialization and Convex… · p. 2
  • Sensor-network localization relaxations use SDP or SOCP methods for ranges and point positions, whereas SCORE combines range and rigid-transformation measurements to estimate poses.The cited sensor-network relaxations consider only ranges and estimate Euclidean point positions.Source: B. Initialization and Convex… · p. 2

C. Novelty of Our Approach

SCORE combines rotation and range relaxations in a single approach for RA-SLAM. The surrounding sections establish notation and introduce the graph-based problem model.

C. Novelty of Our Approach

  • SCORE relaxes rotations as in and range measurements as in, jointly handling relative-pose and range measurements while generalizing both approaches.It derives the relaxation from the full MAP problem and jointly initializes all RA-SLAM variables; it also generalizes to arbitrary dimensions and multi-robot cases.Source: C. Novelty of Our Approach · p. 2

III. NOTATION AND MATHEMATICAL PRELIMINARIES

  • The mathematical presentation uses d-dimensional space, with Id for the identity matrix, SO(d) for the special orthogonal group, and tildes to mark noisy measurements.It defines vector 2-norm and Frobenius norm notation and introduces Gaussian and isotropic Langevin distributions.Source: §III · pp. 2–3

IV. RANGE-AIDED SLAM FORMULATION

  • The paper models RA-SLAM as a graph-based inference problem and derives its corresponding MAP estimation problem.The supplied passage introduces this section's modeling and estimation objective.Source: §IV · p. 3

A. Inference Over a Graph

RA-SLAM is represented as graph inference over pose and landmark variables linked by relative-transformation and range measurements. The resulting MAP problem is expressed as nonlinear least squares with distinct sources of non-convexity.

A. Inference Over a Graph

  • The inference graph represents robot poses and landmark positions as nodes, with separate edge sets for relative rigid transformations and measured distances.Each relative-transformation edge records a measurement from one node to another; range edges likewise encode a measured distance between nodes.Source: A. Inference Over a Graph · p. 3

B. Measurement Noise Models

  • Relative-transform measurements model translation and rotation noise, while the paper assumes a Gaussian generative model for range measurements.The relative rotation is defined from the nodes' orientations, and relative translation from their positions in the first node's frame; isotropic Langevin noise is assumed for presentation but the approach generalizes to anisotropic Langevin noise.Source: B. Measurement Noise Models · p. 3, C. MAP Estimation for RA-SLAM · p. 3

C. MAP Estimation for RA-SLAM

  • The MAP estimate is expressed as nonlinear least squares, with relative-transformation terms and a separate range-measurement term.The rotation variables lie in the non-convex set SO(d), and range costs are also non-convex because they include ∥ti − tj∥2.Source: C. MAP Estimation for RA-SLAM · p. 3

V. SCORE: SECOND ORDER CONIC RELAXATION

The section reformulates RA-SLAM’s MAP problem as a QCQP and then relaxes that formulation to an SOCP called SCORE.

V. SCORE: SECOND ORDER CONIC RELAXATION

  • The MAP problem is reformulated as a QCQP before SCORE is constructed as an SOCP relaxation.The QCQP is an intermediate representation; the approach relaxes it rather than solving it directly.Source: §V · p. 3
  • SCORE is derived by relaxing the QCQP formulation of RA-SLAM.Source: §V · p. 3
  • The relaxation produces a convex optimization problem in the form of a second-order cone program.Source: §V · p. 3

A. RA-SLAM as a QCQP

The authors convert the non-convex RA-SLAM problem into a QCQP, then relax constraints to obtain an SOCP that existing solvers can solve globally.

A. RA-SLAM as a QCQP

  • The QCQP reformulation removes the determinant constraint and introduces auxiliary range variables to make the cost terms quadratic.Its constraints are quadratic equalities; apart from the determinant constraint, it is an exact reformulation of the original problem.Source: A. RA-SLAM as a QCQP · p. 3

B. Relaxing Equation (6) to a SOCP

  • SCORE is derived by relaxing the QCQP; the QCQP itself is not solved because it is not computationally practical.Source: B. Relaxing Equation (6) to a SOCP · p. 4
  • Removing orthonormality constraints and replacing auxiliary-variable equalities with convex inequalities yields second-order cone constraints.The inequalities relate each auxiliary distance variable to the norm of the difference between position variables.Source: B. Relaxing Equation (6) to a SOCP · p. 4
  • The resulting SOCP can be solved to global optimality by existing solvers, with the first rotation constrained to prevent the trivial all-zero solution.Source: B. Relaxing Equation (6) to a SOCP · p. 4

VI. USING SCORE TO INITIALIZE LOCAL-OPTIMIZATION

SCORE estimates relaxed poses and landmarks, projects the rotations back to valid rotation matrices, and uses that solution to initialize local MAP optimization.

VI. USING SCORE TO INITIALIZE LOCAL-OPTIMIZATION

  • Because relaxed constraints can make SCORE’s solution differ from the original problem’s solution, the estimate is projected onto the original feasible set before local optimization.The projection involves the estimated rotations, which may not be valid rotation matrices; singular value decomposition finds the nearest rotation matrices.Source: §VI · p. 4
  • After projecting rotations, the method has valid pose and landmark estimates for initializing the MAP problem and refining them with an iterative local NLS solver.Source: §VI · p. 4

VII. EXPERIMENTS

  • The evaluation covers three real-world AUV trials, simulated single-robot cases, and simulated multi-robot cases with interrobot ranging.The comparisons use MAP estimates from different initializations; quantitative analysis uses absolute pose error against ground truth.Source: §VII · p. 4, A. Experimental Evaluation · p. 4

B. Real-World AUV Experiments

The experiments use AUV data and simulated RA-SLAM cases to assess SCORE initialization against odometry and other initialization strategies.

1) Single-Robot Odometry Initialization (Odom):

  • The AUV odometry initialization set the first pose to identity and composed noisy odometry to estimate later poses.The static beacon initialization used a random sample from the environment.Source: 1) Single-Robot Odometry… · p. 4
  • SCORE initialization consistently produced lower APE than odometric initialization in the three AUV trials.The AUV data used four acoustic beacons; the ground-truth trajectory was obtained by MAP estimation using GPS for the initial pose, odometry, acoustic ranging, and known beacon locations.Source: B. Real-World AUV Experiments · p. 4, 1) Single-Robot Odometry… · p. 5

C. Generation of Simulations

  • The simulation generation placed robots on a grid and gave each robot a 50% chance per time step of generating a range measurement to another robot or beacon.Source: C. Generation of Simulations · p. 5

D. Simulated Single-Robot Experiments

The simulated single-robot experiments test SCORE with one robot and one static ranging beacon. They compare SCORE with odometry and ground-truth initialization across repeated trials.

D. Simulated Single-Robot Experiments

1) Evaluation of Single-Robot Experiments:

In multi-robot simulations, SCORE has APE similar to GT-Init under high noise and lower APE than odometric initialization in both noise settings. The experiments also compare SCORE with GT-Init and odometry in single-robot trials.

1) Evaluation of Single-Robot Experiments:

  • In single-robot trials, SCORE and GT-Init yield effectively the same APE, while odometry produces notably worse solutions, with APE increases up to two orders of magnitude.Source: 1) Evaluation of Single-Robot… · p. 6

E. Multi-Robot Experiments

1) Multi-Robot Initialization:

  • GT-Init is an idealized upper bound; Odom-P assumes each robot’s first pose is known, whereas Odom-R starts each robot randomly with no prior information.Source: 1) Multi-Robot Initialization · p. 6

2) Evaluation of Multi-Robot Experiments:

VIII. DISCUSSION

The discussion reports strong performance across real-world and simulated RA-SLAM problems, while noting that some multi-robot SCORE initializations fail. The conclusion presents SCORE as an SOCP relaxation and connects its performance to empirical conditions and hypotheses.

VIII. DISCUSSION

  • Some refined estimates remain high quality despite compressed translations; the authors hypothesize good initial orientations and beacon locations contribute to this outcome.Source: §VIII · p. 7

A. When Does SCORE Return Poor Initializations?

VIII. DISCUSSION

  • Across the evaluated problems, SCORE outperforms state-of-the-art initialization techniques, with up to two orders of magnitude lower average pose error than odometry-based initializations.Source: §VIII · pp. 6–7, §IX · p. 7

IX. CONCLUSIONS

  • The conclusion also presents a QCQP formulation of RA-SLAM as an independent contribution and connects it to broader work.Source: §IX · p. 7
Loading 2210.03177v1…