Multi-Robot Localization and Target Tracking with Connectivity Maintenance and Collision Avoidance Rahul Zahroof1 Jiazhen Liu1 Lifeng Zhou2 Vijay Kumar1

2025-04-29 0 0 670.67KB 8 页 10玖币

侵权投诉

Multi-Robot Localization and Target Tracking with

Connectivity Maintenance and Collision Avoidance

Rahul Zahroof*1, Jiazhen Liu*1, Lifeng Zhou2, Vijay Kumar1

Abstract— We study the problem that requires a team

of robots to perform joint localization and target tracking

task while ensuring team connectivity and collision avoidance.

The problem can be formalized as a nonlinear, non-convex

optimization program, which is typically hard to solve. To

this end, we design a two-staged approach that utilizes a

greedy algorithm to optimize the joint localization and target

tracking performance and applies control barrier functions

to ensure safety constraints, i.e., maintaining connectivity of

the robot team and preventing inter-robot collisions. Simulated

Gazebo experiments verify the effectiveness of the proposed

approach. We further compare our greedy algorithm to a non-

linear optimization solver and a random algorithm, in terms

of the joint localization and tracking quality as well as the

computation time. The results demonstrate that our greedy

algorithm achieves high task quality and runs efﬁciently.

I. INTRODUCTION

Multi-robot systems are attracting increasing research at-

tention due to their wide applications in ﬁelds such as search

and rescue [1], environment monitoring [2], exploration [3],

and many more. In most applications, the multi-robot team

is equipped with a suite of sensors to perform team-level

tasks. To optimize the task performance, the robots need

to actively reconﬁgure their positions as well as coordinate

with each other. The speciﬁc task motivating this paper is

multi-robot multi-target tracking, or using multiple robots

to track the positions of multiple targets. In contrast to the

sole problem of target tracking, which generally assumes

the true positions of robots to be known a priori [4]–[7],

our setting requires estimating both the robots’ and targets’

positions using sensors mounted on the robots. This joint task

of localization and target tracking is further complicated by

the fact that the robots often have a limited communication

range. If a certain robot is not within the communication

range of any of its teammates, localization and tracking

performance would deteriorate since the overall estimation

accuracy heavily depends on the robots exchanging informa-

tion with each other. For instance, a robot out of contact with

other teammates may suffer from poor localization. Due to

the lack of knowledge about its accurate position, the robot’s

target tracking performance may also degrade. Therefore, the

communication network formed by the robot team should

*Equally contributed.

1The authors are are with the GRASP Laboratory, University of Penn-

sylvania, Philadelphia, PA 19104, USA (email: {rahulz, jzliu,

kumar}@seas.upenn.edu).

2The author is with the Department of Electrical and Computer

Engineering, Drexel University, Philadelphia, PA 19104, USA (email:

lz457@drexel.edu).

This research was sponsored by the Army Research Lab through ARL

DCIST CRA W911NF-17-2-0181.

always remain connected. Meanwhile, inter-robot collision

avoidance should be avoided by ensuring that the robots

maintain a minimum safety distance between each other.

Our major contributions are the formulation of the joint

problem of localization and target tracking as a nonlinear,

nonconvex optimization program and the development of a

two-staged approach for solving the program. In the ﬁrst

stage, we design a greedy algorithm that optimizes the

performance of joint localization and multi-target tracking

without considering any constraints. Then in the second

stage, we leverage control barrier functions (CBFs) to en-

sure safety constraints such as connectivity maintenance

and collision avoidance. The proposed greedy algorithm

achieves high performance on the joint task. Furthermore,

it runs in polynomial time and thus favorably scales up to

larger team sizes. The upcoming sections are arranged as

follows. Section II grounds our work on the foundation of

previous literature. Section III details notation conventions

and formal deﬁnitions for the joint self-localization and target

tracking problem. The main greedy algorithm, along with our

methods for maintaining team connectivity and computing

estimates, is described in Sec. IV. We present both qualitative

illustrations and quantitative comparisons in Sec. V. Finally,

Sec. VI concludes the paper and proposes several future

extensions.

II. RELATED WORK

The joint task of self-localization and target tracking has

been previously addressed by a sizable amount of litera-

ture [8]–[11]. Concretely, [8] focuses on the case where the

association between target measurements and target identities

is unknown. A novel decentralized method is proposed to

deal with an unknown and time-varying number of tar-

gets under association uncertainty. The generalized approach

proposed in [9] for joint self-localization and tracking of

generic 3D objects is applicable to any type of environment.

In [10], the self-localization problem is cast as a static

parameter estimation problem for Hidden Markov Models.

Decentralized adaptations of the Recursive Maximum Like-

lihood and online Expectation-Maximization algorithms are

used to address self-localization along with target tracking.

These works primarily focus on algorithm design for either

localization or tracking, leaving out safety guarantees such

as network connectivity maintenance, which is an essential

component if a multi-robot team is to be deployed for

executing practical tasks. Moreover, considering that self-

localization and target tracking could both be seen as iterative

state estimation problems, various ﬁltering algorithms have

arXiv:2210.03300v2 [cs.RO] 10 Oct 2022

been applied to deal with uncertainty in the estimates [12]–

[14]. Localization and target tracking have also been explored

in more challenging scenarios with limited GPS [15]–[17].

Besides pursuing accurate localization and target tracking,

safety-critical constraints such as connectivity maintenance

and collision avoidance should be properly considered. To

this end, [18] proposes a CBF for connectivity maintenance.

It provides an elaborate discussion on the CBF and its rela-

tionship with Lyapunov control. In [19], the connected region

is formulated as a safety set, and the CBF is utilized to render

the set forward invariant. This guarantees the connectivity of

the network throughout the duration of the task as long as

the robots are initially connected. The work [20] approaches

connectivity maintenance by considering spectral graph prop-

erties such as algebraic connectivity. In [21], control actions

are designed according to the weights assigned to graph

edges in order to ensure connectivity.

III. PROBLEM FORMULATION

We consider that a team of robots is tasked to track

multiple dynamic targets. We assume the prior position

estimates of all robots and targets to be known. However,

such prior knowledge is noisy and not accurate. The goal

of the robots is to actively reconﬁgure their positions to

minimize the uncertainties originating from target tracking

and localization, while maintaining team connectivity and

avoiding collisions. In this section, we ﬁrst describe the

notations to be used throughout the paper, then introduce the

modeling of robots and targets, and ﬁnally deﬁne the joint

task of target tracking and localization as an optimization

problem.

A. Notation

Capital letters in boldface are used to denote matrices;

lower-case letters in boldface represent vectors; lower-case

letters of regular font are scalars. Subscripts are used to

indicate the relevant group (the set of targets denoted by

T, robots denoted by R), measurement type, and vec-

tor/matrix indices. Superscripts indicate the time step at

which a variable is computed. The overhead bar indicates

a prior estimate, and the overhead hat indicates a posterior

estimate. Vectors and matrices described as sequences of

vectors/matrices in the form M= [M1,M2,· · · ,MN]

imply horizontal concatenation. The matrix format M=

blockdiag (M1,M2,· · · ,MN)describes a block-diagonal

construction of matrix Mwith the listed matrices placed

along its diagonal. k·k2represents 2-norm of vectors.

B. Modeling of Robots and Targets

a) Robots: We consider a team of Nrobots, indexed

by R={1,· · · , N}. Each robot i∈ R has the following

discrete motion model:

Ri=ARixt−1

Ri+BRiut−1

Ri+GRiwt−1

d,Ri,(1)

where tdenotes the current time step and ARiand BRiare

the process and control matrices, respectively. The term uRi

denotes the control input for robot i.GRiis the gain matrix

to amplify noise for robot i, and wdis a white Gaussian

noise with zero mean and covariance Qt

d=E[wt

d(wt

d)T].

b) Targets: We consider Mtargets, indexed by T=

{1,· · · , M}. Each target i∈ T has the following discrete

motion model:

Ti=ATixt−1

Ti+BTiut−1

Ti+GTiwt−1

d,Ti,(2)

where the control input uTiis predeﬁned.

c) Sensing: We consider that each robot i∈ R makes

observations of each target j∈ T according to the following

measurement model:

Ri,Tj=hxt

Ri,xt

Tj+nt

Ri,Tj,(3)

where his the measurement function (e.g., range and bear-

ing measurement) and nt

Ri,Tjis a zero-mean white Gaus-

sian noise term with covariance RRi,Tj. The measurement

function h, the measurement noise nt

Ri,Tj, and the noise

covariance RRi,Tjall depend on the states of the robots and

targets. Each robot i∈ R also makes observations of itself

and every other robot j∈ R, j 6=iusing the same model:

Ri,Rj=hxt

Ri,xt

Rj+nt

Ri,Rj,(4)

where nt

Ri,Rjis a zero-mean white Gaussian noise term

with covariance RRi,Rj. Self-observation relying on GPS

is denoted by zt

Ri,Riwith a zero-mean white Gaussian noise

term nt

Ri,Riwhich has covariance RRi,Ri.

d) Communication: Each robot communicates only

with the robots within a prescribed communication range rc.

We assume all robots to have the same communication range

rc. Based on this range, an (undirected) communication

graph Gt={R,Et}is induced at step t, with the robots

Ras nodes. Edges Etbetween nodes are deﬁned such that

(i, j)∈ Etif and only if kxt

Ri−xt

Rjk2≤rc. We use Lt

and λt

2to denote the weighted Laplacian matrix of Gtand

the second smallest eigenvalue of Lt, respectively.

C. Problem Deﬁnition

The problem of optimizing the quality of joint target

tracking and localization while ensuring team connectivity

and collision avoidance can be formally deﬁned as the

following optimization program:

min

ut−1

Tr(ˆ

Pt)

s.t.λt

2>0,

kxt

Ri−xt

Rjk2≥dmin,∀i, j ∈ R,

kut−1

Rik2< umax,∀i∈ R.

(5)

The objective is to minimize the uncertainty in the posi-

tions of both the robots and targets. We quantify the uncer-

tainty as the trace of the joint posterior covariance matrix of

the robots and targets, denoted by ˆ

Pt=blockdiag(ˆ

R,ˆ

with ˆ

Rand ˆ

Tbeing the covariance matrices of the robots

and targets, respectively. In addition, the program includes

three constraints. The ﬁrst constraint ensures the connectivity

of robots by making the second smallest eigenvalue larger

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

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

10 玖币 0人已下载

立即下载

摘要：

Multi-RobotLocalizationandTargetTrackingwithConnectivityMaintenanceandCollisionAvoidanceRahulZahroof*1,JiazhenLiu*1,LifengZhou2,VijayKumar1AbstractWestudytheproblemthatrequiresateamofrobotstoperformjointlocalizationandtargettrackingtaskwhileensuringteamconnectivityandcollisionavoidance.Theproblemca...

展开>> 收起<<

Multi-Robot Localization and Target Tracking with Connectivity Maintenance and Collision Avoidance Rahul Zahroof1 Jiazhen Liu1 Lifeng Zhou2 Vijay Kumar1.pdf

共8页,预览2页

还剩页未读，继续阅读

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

Multi-Robot Localization and Target Tracking with Connectivity Maintenance and Collision Avoidance Rahul Zahroof1 Jiazhen Liu1 Lifeng Zhou2 Vijay Kumar1

相关推荐

开通VIP享超值会员特权

作者详情

相关内容

热门标签

举报选择: