SCORE A Second-Order Conic Initialization for Range-Aided SLAM Alan Papalia12 Joseph Morales1 Kevin J. Doherty12 David M. Rosen34 John J. Leonard1 Abstract We present a novel initialization technique for

2025-05-03 0 0 1.23MB 9 页 10玖币

侵权投诉

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

Alan Papalia1,2, Joseph Morales1, Kevin J. Doherty1,2, David M. Rosen3,4, John J. Leonard1

Abstract— 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

ﬁrst convex relaxation for RA-SLAM. We present real-world

and simulated experiments which show SCORE initialization

permits the efﬁcient 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

Range-aided simultaneous localization and mapping (RA-

SLAM) is a key robotic task, with applications in planetary

[1], subterranean [2], and sub-sea [3]–[5] environments.

RA-SLAM combines range measurements (e.g., distances

between acoustic beacons) with measurements of relative

rigid transformations (e.g., odometry) to estimate a set of

robot poses and landmark positions.

RA-SLAM differs from pose-graph simultaneous localiza-

tion and mapping (SLAM) in that the sensing models of

range measurements induce substantial difﬁculties for state-

estimation. However, ranging is a valuable sensor modality

and further developments in RA-SLAM could substantially

advance the state of robot navigation.

The state-of-the-art formulation of SLAM problems is as

maximum a posteriori (MAP) inference, which takes the

form of an optimization problem. However, because robot

orientations are a non-convex set, the MAP problem is non-

convex. Additionally, in RA-SLAM, the measurement mod-

els of range sensing introduce further non-convexities. As a

result, standard approaches to solving these MAP problems

use local-search techniques and can only guarantee locally

optimal solutions. Thus, good initializations to the MAP

problem are key to obtaining quality RA-SLAM solutions.

1Computer Science and Artiﬁcial Intelligence Laboratory (CSAIL),

Massachusetts Institute of Technology (MIT), Cambridge, MA 02139,

USA, {apapalia, jrales, kdoherty, jleonard}@mit.edu

2Department of Applied Ocean Physics & Engineering, Woods Hole

Oceanographic Institution, Woods Hole, MA 02543, USA

3Department of Electrical & Computer Engineering, Northeastern Uni-

versity, Boston, MA 02115, USA, d.rosen@northeastern.edu

4Department of Math, Northeastern University, Boston, MA 02115, USA

Fig. 1. Key concepts. (A) An example multi-robot range-aided SLAM

problem. Red circles are problem variables (i.e., robot poses and beacon

position), green squares are relative pose measurements (e.g., odometry),

and blue squares are distance measurements (e.g., acoustic ranging). (B) A

second-order cone program (SOCP) is derived as a convex relaxation of the

original non-convex nonlinear least-squares (NLS) formulation of the range-

aided SLAM problem. The solution to the SOCP is then used to obtain the

initial estimate for the NLS problem, and the NLS problem is solved to

reﬁne the SOCP initialization. (C) Illustration of contributions. The range-

aided SLAM problem is ﬁrst represented as a non-convex NLS problem.

This work shows how to reformulate the NLS problem into a non-convex

quadratically constrained quadratic program (QCQP) and how to relax the

QCQP into a convex SOCP. The SOCP can be solved to obtain an initial

estimate for the original NLS problem.

This work approaches initialization for the non-convex

MAP estimation problem through convex relaxation [6],

the construction of a convex optimization problem which

attempts to approximate the original problem. Convex prob-

lems can be efﬁciently solved to global optimality, and

many convex relaxations have shown success as initialization

strategies in related robotic problems [7]–[10].

This work presents SCORE as a novel methodology for

initializing RA-SLAM problems. SCORE applies to 2D

and 3D scenarios with arbitrary numbers of robots and

landmarks. As SCORE is a SOCP, it is easily implemented

in existing convex optimization libraries, and scales grace-

fully to large problems. We summarize our contributions as

follows:

•A novel, convex-relaxation approach to initializing RA-

SLAM problems.

•The ﬁrst QCQP formulation of RA-SLAM, connecting

RA-SLAM to a broader body of work [8]–[11].

•Our implementation of SCORE is open-sourced1.

1https://github.com/MarineRoboticsGroup/score

arXiv:2210.03177v1 [cs.RO] 6 Oct 2022

II. RELATED WORK

The current state-of-the-art formulation of RA-SLAM is

through nonlinear least-squares (NLS) optimization based

upon MAP inference [12]. This work speciﬁcally focuses on

initialization for this MAP formulation. Though we focus

on MAP estimation, there are many important previous

formulations of RA-SLAM using: extended Kalman ﬁlters

[3], [13]–[15], particle ﬁlters [16], [17], mixture models [18],

and NLS optimization [1], [2], [19].

We ﬁrst survey previous initialization strategies for RA-

SLAM problems and then expand the scope to cover initial-

ization strategies for general pose-graph SLAM problems.

As our initialization approach is a convex relaxation of the

RA-SLAM problem, we discuss related convex relaxations

in robotic state-estimation. Finally, as the key challenge

that differentiates RA-SLAM from pose-graph SLAM is

the nature of range measurements, we discuss the closely

related ﬁeld of sensor network localization (SNL) and convex

relaxations developed speciﬁcally for SNL problems.

A. Initialization in RA-SLAM

Notable work in the single-robot RA-SLAM case used

spectral graph clustering to initially estimate beacon loca-

tions [20]. Similarly, [21] used spectral decomposition to

jointly estimate robot poses and beacon locations. However,

these approaches [20], [21] do not account for sensor noise

models and do not readily extend to multi-robot scenar-

ios. Other initializations for multi-robot RA-SLAM used

coordinated movement patterns to ﬁnd the (single) relative

transform between robots [22], [23].

Finally, a series of works developed increasingly sophis-

ticated methods to compute relative transforms between two

robots using range and odometry measurements [24]–[30].

While these approaches represent great progress, they are

only capable of solving for a single inter-robot relative

transform and rely on (noisy) odometric pose composition

to initialize all other poses. In fact, [28]–[30] utilize convex

relaxation to solve the relative transformation problem.

B. Initialization and Convex Relaxation in State-Estimation

In pose-graph SLAM many existing initializations solve

approximations of the MAP problem. [7] leveraged the

relationships between rotation and translation measurements

in SLAM and explored the use of several rotation averaging

algorithms [31]–[34] to obtain initial estimates. The same

relationships were used to approximate the SLAM problem

as two sequential linear estimation problems [35]. However,

these approaches do not consider range measurements and

thus do not extend to RA-SLAM problems.

With exception to [34], these initialization procedures rep-

resent convex relaxations of the pose-graph SLAM problem.

Subsequent works in pose-graph SLAM developed convex

relaxations which obtained exact solutions to the non-convex

MAP problem [8], [11], [36]–[38]. Other works developed

exact convex relaxations for the problems of extrinsic cali-

bration [9], two-view relative pose estimation [39], [40], and

spline-based trajectory estimation from range measurements

with known beacon locations [41]. With exception to [41],

these convex relaxations also do not extend to range mea-

surements. However, [41] required known beacon locations,

allowed only for range measurements, and only considered

single-robot localization. SCORE allows for multi-robot RA-

SLAM, and combines measurements of rigid transformations

and ranges without necessitating any information a priori.

A number of convex relaxations exist for sensor network

localization (SNL). As SNL centers around point-to-point

distance measurements, these works are closely related to

SCORE. Previous works developed and analyzed semidef-

inite program (SDP) [10], [42], [43] and SOCP [44]–[47]

relaxations of the SNL problem. Whereas these relaxations

only consider range measurements and can only estimate

the Euclidean position of points, SCORE allows for rigid

transformation measurements and estimation of poses. Ad-

ditionally, as SCORE is a SOCP, it is substantially more

scalable than these SDP relaxations.

C. Novelty of Our Approach

SCORE is similar to [31] in its relaxation of rotations and

to [45] in its relaxation of range measurements. However,

it differs from these works in that it jointly considers both

relative pose and range measurements. Notably, SCORE

generalizes the chordal-initialization of [31] and the SOCP

relaxation of [45].

Additionally, there is much commonality to [28]–[30]

in that the convex relaxation of SCORE considers both

range and relative transformation measurements. Unlike

these works, SCORE generalizes to arbitrary dimensions

(e.g. 2- or 3-D) and multi-robot cases. Furthermore, SCORE

differs in that the convex relaxation is derived from the full

MAP problem, and thus utilizes all measurements and jointly

solves for an initialization of all RA-SLAM variables.

III. NOTATION AND MATHEMATICAL PRELIMINARIES

Symbols: This work generalizes across dimensionalities

(e.g., 2-D or 3-D). As such the mathematical presentation

will assume to be working in d-dimensional space. We

denote the d-dimensional identity matrix as Idand the special

orthogonal group as SO(d). We refer to the set of real values

as Rand the set of nonnegative reals as R+. Furthermore,

we indicate vectors and scalars with lowercase symbols (e.g.,

t) and matrices with uppercase symbols (e.g., R). Noisy

measurements are indicated with a tilde (e.g., ˜

R). The true

(latent) value of a quantity will be underlined (e.g., R)

Mathematical Operators: We denote the matrix trace as

tr(·). A diagonal matrix with diagonal elements [a1, . . . , an]

is indicated as diag([a1, . . . , an]). The vector 2-norm is

indicated as k·k2and the Frobenius norm is indicated

as k · kF. We note the following relationship between the

Frobenius norm and trace operators: kAk2

F= tr(AA>) =

tr(A>A).

Probability Distributions: We indicate a Gaussian prob-

ability distribution over Euclidean space, with mean µ∈

Rdand variance Σ∈Rd×das N(µ, Σ). Similarly, we

文档加载中……请稍候！
如果长时间未打开，您也可以点击刷新试试。

下载文档到电脑，查找使用更方便

10 玖币 0人已下载

立即下载

摘要：

SCORE:ASecond-OrderConicInitializationforRange-AidedSLAMAlanPapalia1;2,JosephMorales1,KevinJ.Doherty1;2,DavidM.Rosen3;4,JohnJ.Leonard1AbstractWepresentanovelinitializationtechniquefortherange-aidedsimultaneouslocalizationandmapping(RA-SLAM)problem.InRA-SLAMweconsidermeasurementsofpoint-to-pointdist...

展开>> 收起<<

SCORE A Second-Order Conic Initialization for Range-Aided SLAM Alan Papalia12 Joseph Morales1 Kevin J. Doherty12 David M. Rosen34 John J. Leonard1 Abstract We present a novel initialization technique for.pdf

共9页,预览2页

还剩页未读，继续阅读

声明：本站为文档C2C交易模式，即用户上传的文档直接被用户下载，本站只是中间服务平台，本站所有文档下载所得的收益归上传人(含作者)所有。玖贝云文库仅提供信息存储空间，仅对用户上传内容的表现方式做保护处理，对上载内容本身不做任何修改或编辑。若文档所含内容侵犯了您的版权或隐私，请立即通知玖贝云文库，我们立即给予删除！

SCORE A Second-Order Conic Initialization for Range-Aided SLAM Alan Papalia12 Joseph Morales1 Kevin J. Doherty12 David M. Rosen34 John J. Leonard1 Abstract We present a novel initialization technique for

相关推荐

开通VIP享超值会员特权

作者详情

相关内容

热门标签

举报选择: