Article

Probabilistic cooperative mobile robotarea coverage and its application toautonomous seabed mapping

The International Journal of

Robotics Research

2017, Vol. 37(1) 21–45

© The Author(s) 2018Reprints and permissions:

sagepub.co.uk/journalsPermissions.nav

DOI: 10.1177/0278364917741969

journals.sagepub.com/home/ijr

Liam Paull1,2, Mae Seto3, John J. Leonard1 and Howard Li4

Abstract

There are many applications that require mobile robots to autonomously cover an entire area with a sensor or end effec-

tor. The vast majority of the literature on this subject is focused on addressing path planning for area coverage under

the assumption that the robot’s pose is known or that error is bounded. In this work, we remove this assumption and

develop a completely probabilistic representation of coverage. We show that coverage is guaranteed as long as the robot

pose estimates are consistent, a much milder assumption than zero or bounded error. After formally connecting robot

sensor uncertainty with area coverage, we propose an adaptive sliding window filter pose estimator that provides a close

approximation to the full maximum a posteriori estimate with a computation cost that is bounded over time. Subsequently,

an adaptive planning strategy is presented that automatically exploits conditions of low vehicle uncertainty to more effi-

ciently cover an area. We further extend this approach to the multi-robot case where robots can communicate through a

(possibly faulty and low-bandwidth) channel and make relative measurements of one another. In this case, area coverage

is achieved more quickly since the uncertainty over the robots’ trajectories is reduced. We apply the framework to the

scenario of mapping an area of seabed with an autonomous underwater vehicle. Experimental results support the claim

that our method achieves guaranteed complete coverage notwithstanding poor navigational sensors and that resulting

path lengths required to cover the entire area are shortest using the proposed cooperative and adaptive approach.

Keywords

Area coverage, cooperative localization, marine robotics

1. Introduction

One particularly useful class of tasks for a mobile robot is

to cover an entire area with a sensor or an end effector.

Instantiations from this class can be tedious or dangerous

for humans. Examples include: exploration for mapping

(Kim and Eustice, 2015), autonomous lawnmowing (Bosse

et al., 2007), robotic cleaning (Goel et al., 2013), and seabed

mapping with an autonomous underwater vehicle (AUV)

(Galceran et al., 2014; Paull et al., 2013).

A wide range of literature has tackled the area coverage

planning problem (for a recent survey, see Galceran and

Carreras, 2013). The vast majority of these methods aim to

produce provably complete paths and assume that the robot

is able to follow the desired path exactly (Acar et al., 2006;

Acar and Choset, 2002b; Acar et al., 2002, 2003; Baek

et al., 2011; Choset, 2000; Englot and Hover, 2010; Guo and

Balakrishnan, 2006; Jin and Tang, 2011; Lee et al., 2011; Li

et al., 2011; Mannadiar and Rekleitis, 2010; Oksanen and

Visala, 2009; Xu et al., 2011; Yang and Luo, 2004). How-

ever, in many practical cases there can be divergence from

the planned path and actual path for a number of reasons:

(1) errors in path tracking; (2) dynamics constraints of the

vehicle; (3) error in the robot’s own pose estimate resulting

from noisy sensor data being used for localization.

For example, consider Figure 1, which shows field data

of an AUV trying to cover an area of seabed with a side-

scan sonar (SSS) and a simple “lawnmower” path planning

approach (see Section 6 for more details). In this case, the

vehicle is dead-reckoning (DR) based on noisy proprio-

ceptive velocity measurements from an onboard Doppler

velocity log (DVL) and a three-axis compass using a simple

extended Kalman filter (EKF). When the vehicle leaves the

area of interest, it surfaces for a GPS fix as can be noted

1Computer Science and Artificial Intelligence Laboratory (CSAIL), MIT,

Cambridge, MA, USA2Département d’informatique et de recherche opérationnelle (DIRO),

Université de Montréal, Montréal, Québec, Canada3Defense R&D Canada, Dartmouth, Nova Scotia, Canada4Department of Electrical Engineering, University of New Brunswick,

New Brunswick, Canada

Corresponding author:

Liam Paull, Département d’informatique et de recherche opérationnelle

(DIRO), Université de Montréal, 2920 chemin de la Tour, Montréal,

Québec, Canada.

Email: paulll@iro.umontreal.ca

22 The International Journal of Robotics Research 37(1)

Fig. 1. Top: An autonomous underwater vehicle (AUV) in the

water at the test facility in Dartmouth, NS. The AUV submerges

and then uses its side-scan sonars (SSSs) to map the seabed. This

specific use-case is applied to the general probabilistic area cover-

age framework in Section 6. Bottom: In an experiment the vehicle

is run at the surface and the GPS data is blocked while the vehi-

cle is inside the workspace to simulate submergence. Deviation

from the planned path results in significant areas of the workspace

being missed. (a) Plot of the environment to be covered (yellow)

with desired tracks (green), estimated path (red), and actual path

(blue). (b) Desired coverage. (c) Estimated coverage from output

of then extended Kalman filter (EKF). (d) Actual coverage based

on GPS data.

by a jump in the estimate of the red track in Figure 1(a).

Figure 1(b), (c), and (d) show the desired, perceived, and

actual coverage obtained, respectively. The discrepancy

between desired (b) and perceived (c) coverage is caused by

the vehicle’s inability to exactly follow the prescribed path

due to external disturbances (in this case water currents).

The discrepancy between perceived (c) and actual (d) cov-

erage is caused by inaccuracy in the vehicle state estimate

as a result of noisy localization sensor data.

The area coverage problem presents challenges not

addressed previously in closely related topics. Planning for

area coverage with uncertainty is fundamentally different

to the general navigation under uncertainty problem, since

the area covered is a function of the entire trajectory of the

robot. Robot exploration and mapping are also related (Kim

and Eustice, 2015), but for area coverage we cannot assume

that the level of coverage or the boundaries of the workspace

can be directly sensed: instead we rely entirely on a model

of the coverage sensor or end effector.1

Recent work on “robust” area coverage to addresses the

issue of localization uncertainty (Bretl and Hutchinson,

2013; Das et al., 2011; Goel et al., 2013) through strict

bounds on localization error in order to produce coverage

guarantees and tend to produce plans that are overly con-

servative and consequently highly sub-optimal (for a more

detailed review of these works see the related work in Sec-

tion 8). Here, we propose a general approach to area cover-

age that explicitly accounts for all sources of uncertainty.

The framework models the coverage over the workspace

probabilistically by propagating the vehicle pose uncer-

tainty to the coverage sensor frame and finally to the world

frame by mapping through a general coverage sensor func-

tion. As a result, we are able to produce feedback planning

policies that are much less conservative by relaxing the

requirement that state error be bounded to the requirement

that the state estimate is consistent, meaning that the statis-

tics used to parameterize the estimated distribution accu-

rately represent the true statistics. The planning policy is

adaptive to the most up-to-date vehicle trajectory estimate

(as provided by a maximum a posteriori estimator) and

is consequently able to exploit conditions of better robot

localization to cover area more efficiently, without violating

conditions for complete coverage guarantee.

1.1. Coverage with multiple robots

The area coverage problem is an ideal candidate for multi-

robot systems since it is inherently parallelizable. In a naive

implementation,2 a team of N robots should be able to cover

an area A roughly N times faster if the area to be covered

can be easily decomposed into sub-areas of size A/N .

It is known that if robots can communicate and make

measurements of their relative positions, then they can

reduce the uncertainty of their respective pose estimates

(Roumeliotis and Bekey, 2002), or even full trajectories

(Nerurkar et al., 2009). This can result in further poten-

tial benefits for the multi-robot approach to area coverage.

However, in many scenarios such as in space, under-

water (Paull et al., 2014b), and in humanitarian disas-

ters (Iagnemma and Overholt, 2015), the communications

environment may be severely restricted, intermittent, and

unreliable. We propose a decentralized trajectory estimation

scheme based on measurement composition and bookkeep-

ing that is able to operate with very poor communication

links. We show that this ability to cooperatively estimate

trajectories has real tangible benefits in terms of reducing

the lengths of paths required to cover areas.

1.2. Seabed mapping with AUVs

Seabed mapping with an AUV is an instance of an area

coverage problem, since the vehicle must pass its sensor

Paull et al. 23

swath over every point on the seabed. However, it is par-

ticularly challenging since the performance of the coverage

sensor (ability to identify mines in the gathered imagery)

is non-linear and range dependent, and the uncertainty of

the sensor position will grow as the AUV traverses under-

water in the absence of any global position reference such

as pre-installed beacons (Kinsey et al., 2006; Paull et al.,

2014a).

Although the decentralized coverage framework pre-

sented here is generally applicable to area coverage tasks

for robots with significant pose uncertainty, the underwater

case is particularly applicable for two reasons.

1. Localization error, in general, cannot be considered

negligible and grows with time. The ability to provide

better guarantees about the actual area covered has tan-

gible benefits for these types of missions since the vehi-

cle will have to surface less often for GPS fixes, which

saves time and effort, and the mission can be performed

with less-expensive on-board navigation sensors.

2. Underwater cooperative localization is realizable

through acoustic communications. Although com-

municating through acoustics is low-bandwidth and

unreliable, each data packet induces a relative range

measurement in the case that the vehicles have precisely

synchronized on-board clocks.

We apply the proposed approach to the underwater seabed

mapping domain in Section 6.

1.3. Summary of contributions

The work presented here builds on the following previous

articles published by the present authors.

1. In Paull et al. (2014c) we presented the probabilis-

tic coverage framework for AUVs. This approach has

the following advantages over related work in the lit-

erature: (1) it makes no assumptions on the bound-

edness of location uncertainty; (2) it is applicable to

general coverage sensor geometries; and (3) it admits

general coverage sensor functions (as opposed previous

approaches that only consider uniform coverage sen-

sors). This work also presents a preliminary adaptive

planner for coverage.

2. In Paull et al. (2014b) we presented a bandwidth-

constrained cooperative trajectory estimation frame-

work that is suitable for faulty and bandwidth-limited

communication links.

This paper supersedes and extends our previous work in the

following ways (which we claim as our contributions here).

• We generalize the probabilistic coverage framework

from Paull et al. (2014c) to encapsulate all cover-

age problems. In addition, we formalize the conditions

under which probabilistic coverage guarantees actual

coverage (Theorem 1 in Section 2).

• We motivate the need for a trajectory (as opposed to a

state) estimation scheme for more efficient coverage.

• We extend Paull et al. (2014b) to include a sliding

window to bound state space growth.

• We improve the adaptive controller from Paull et al.

(2014c) to perform real-time track spacing updates (as

opposed to optimizing track spacing after each track has

been followed).

• We demonstrate with field experiments how all of these

components can be integrated into one system and that

the resulting system performs better on real underwater

vehicle hardware in terms of the path lengths required

to guarantee complete coverage.

1.4. Outline

The remainder of the paper is structured as follows. We

begin by presenting the probabilistic area coverage frame-

work in Section 2. We present the constant-time scaling

adaptive sliding window filter in Section 3. The decen-

tralized cooperative MAP estimation scheme is presented

in Section 4. The adaptive uncertainty-aware planning

approach is presented in Section 5. The application to the

task of cooperative seabed scanning for mine countermea-

sures is presented in Section 6. Results are presented in

Section 7. Some related work is summarized in Section 8

and, finally, conclusions are presented in Section 9.

2. General probabilistic area coverage

framework

We begin by formalizing the coverage problem assuming

known robot poses. This assumption is then relaxed to con-

nect coverage estimation with robot navigation. We show

how the coverage representation can be iteratively main-

tained when the robot state estimate is obtained through

filtering. Our procedure is first illustrated with a simple

coverage problem, the robotic vacuum. We then relax the

requirement that the coverage sensor performance is perfect

and uniform by considering an example where this is not

the case (a lawnmower with uneven blades). Subsequently,

we detail (through Theorem 1) under what conditions cov-

erage in our probabilistic representation guarantees actual

real-world coverage. Finally, we demonstrate how to recur-

sively combine distributions in the case that an area of the

workspace is covered multiple times.

2.1. Preliminaries

Consider a robot with platform geometry A and coverage

sensor swath S (Figure 2). Define the workspace W as the

area that is to be covered.

The workspace is decomposed into n small grid cells:

ci, i = 1, . . . , n, where the size of the cells is sufficiently

small that the coverage can be treated as uniform over the

cell. This cell size could be determined by the resolution

24 The International Journal of Robotics Research 37(1)

Fig. 2. The workspace W is decomposed into n cells. As the

robotic platform, A, moves about, W becomes covered in a

manner consistent with the coverage sensor geometry S.

of the coverage sensor. Each cell ci is represented by its

position in the workspace. For example, for the 2D case,

ci , [xi, yi]T where xi and yi are the x and y locations,

respectively, of cell ci in the global frame.3

If the robot takes a sensor reading at time t, from pose

xt , [xt, yt]T, then the set of all cells that are at least

partially covered by the measurement defines the coverage

sensor swath, St.

The degree of coverage of cell ci is determined by its

location in the coverage sensor frame, which is obtained by

performing a transformation from the global frame to the

sensor frame through the robot body frame:

scit , [sxi

t,syi

t]T = s

rTrgT(xt)

gci = sgT(xt)

gci (1)

where scit and gci are the location of the cell in the sensor

and the global frame, respectively, and baT is a homogeneous

transformation from frame a to frame b. In the absence of

any specified frame (preceding superscript) we assume the

global frame.

We can associate with each cell ci, i = 1, . . . , n, a value

wit that represents the level to which cell ci is covered at time

t. Based on a detailed investigation of the coverage sensor

the cells are updated according to:

wit = H( sci

t) (2)

where H : RN 7→ [0, 1] represents the coverage sensor

model and defines how coverage values are assigned to cells

within the sensor swath.

If we have a uniform and perfect coverage sensor, then

w ∈ {0, 1}, meaning that each cell is either covered or not.4

In this case we state the complete coverage objective as

requiring

witend= 1, ∀i = 1, . . . , n (3)

where tend is the time at the endpoint of the coverage mis-

sion. However, in many cases the coverage sensor perfor-

mance is not perfect. In the case of a non-uniform sensor

characteristic, w ∈ [0, 1], and (3) can become difficult or

Fig. 3. A vacuum cleaning robot moving with drift. The blue

arrow and associated coverage area correspond to the desired path

and coverage. The red arrow and area are the actual path and cov-

erage. Some areas are not covered that were meant to be, while

others were inadvertently covered.

even impossible to achieve depending on the coverage sen-

sor model. We can consider a cell to have been covered if

its coverage value is greater than some specified level:

wit > wth (4)

and the mission to be complete when the proportion of cells

that have been covered is greater than P1:

1

n

∣

∣

∣{i|wi

tend> wth}

∣

∣

∣> P1 (5)

where |{·}| is the cardinality operator on a set. The job of

the typical deterministic coverage path planner is then to

find a path that achieves the objective (5) while minimizing

the time to completion tend , or energy expenditure or some

other metric.

2.2. Coverage with uncertain poses

If we remove the assumption that the coverage sensor is

perfectly localized then the coverage completion criterion

(5) is not sufficient because it does not encapsulate the

uncertainty in the coverage.

Example 1 (Floor cleaning robot). Consider the simple cir-

cular floor cleaning robot in Figure 3. In this case, the size

of the coverage sensor is identical to the robot’s footprint

(S = A). The robot might estimate its position from only

wheel encoders. As a result, the position estimate will drift

from the actual position due to the noise of the encoder

measurements being accumulated through integration. In

Figure 3, the robot drift results in some areas being inad-

vertently covered whereas other areas that were meant to be

covered are not. In Figure 4, we consider the same vehicle

platform at a snapshot in time. In this case, we can be fairly

certain that cell c1 is covered and fairly certain that cell c3

is not covered. However, for cell c2 there is a high degree of

uncertainty about the coverage status.

We define the posterior pose distribution at time t to be

given by bel(xt) in the standard way:

bel(xt) , p(xt|u1:t, z1:t,x0) (6)

wherex is the robot state, u are control inputs or odometry,

and z is a measurement used for localization. This belief is

Paull et al. 25

Fig. 4. The robot from Figure 3 at a snapshot in time. The whiter

areas indicate areas that have a higher probability of being cov-

ered. The cells c1, c2 and c3 are assumed to be uncovered before

this coverage sensor reading. Each of the cells will have different

resulting probabilities of being covered.

generally recursively updated through an approximation to

the Bayes’ filter (Thrun et al., 2005):

¯bel(xt) =

∫

p(xt|xt−1, ut) bel(xt−1) dxt−1

bel(xt) = ηp( zt|xt) ¯bel(xt)

(7)

Note that the sensor producing the localization measure-

ments z may or may not be the same sensor being used for

coverage.

To generate a probabilistic representation of coverage, we

use the entire pose posterior at time t to update the cover-

age estimate. This is achieved through a modification of the

formulation in Section 2.1. Instead of each cell having one

value that represents its coverage, it has a distribution rep-

resenting the probability of coverage to different levels. The

uncertainty in the robot pose transfers to an uncertainty in

the position of cell i in the sensor frame: scit caused by the

uncertain transformation rgT(xt) in (1). The position of cell

i in the sensor frame is now uncertain and is represented by

a RV sCit whose distribution is calculated by mapping the

position of the cell in the global frame through the uncertain

transformation from global to sensor frame:

p( sCit = c)= p( s

gT( Xt)gci = c) , p( sci

t) (8)

The uncertain location of the cell in the sensor frame

results in an uncertain coverage. Consequently, the coverage

is now represented by an RV W it where p(W i

t = w) repre-

sents the probability that cell ci is covered to a coverage

level w at time t considering all past robot states.

It is useful to define an RV W̆ it that represents the proba-

bility that cell ci is covered to a level w resulting from only

the sensor reading at time t.

The robot pose uncertainty will result in unintended cells

having some probability of being covered. To bound the

problem, we define the effective coverage sensor swath, S ′tto be all cells that have a “reasonable” chance of being

covered at time t:

S′t = {c

i | p(ci ∈ St) > ε, i = 1, . . . , n} (9)

Fig. 5. Iterative coverage estimation: at each time-step, the pose

distribution is used to map the coverage cells into the sensor frame

(Section 2.2). These probabilistic cell locations are fed through the

coverage sensor function H to obtain the distribution of coverage

at each cell resulting from the cover at only that timestep (Section

2.3). These coverage distributions are then incorporated with the

previous “covers” of the same cell through the conservative MAX

operation (Section 2.6).

where ε is some arbitrarily small threshold.

We achieve recursive coverage estimation using the pose

beliefs based on the model shown in Figure 5. In the fol-

lowing, we describe how to propagate the pose uncertainty

to the coverage belief at a single timestep in Section 2.3

and how to combine the coverage belief from the previ-

ous timestep with the estimate generated from time t in

Section 2.6. In Figure 5, it is assumed that the coverage

belief is Markovian, an assumption that will be revisited in

Section 2.6.

Given the probabilistic representation of coverage, we

update the definition of complete coverage (5) to encap-

sulate the new uncertainty about the coverage values. The

coverage criterion for a cell (4) is now updated to be that

the probability that a cell is covered to at least a level of wth

is at least P2:

p(W it ≥ wth)≥ P2 (10)

and, similarly to (5), the mission is considered complete if

a proportion of at least P1 of the cells are covered:

1

n

∣

∣{i | p(W it ≥ wth)≥ P2}

∣

∣ > P1 (11)

In the case where the coverage sensor characteristic is

uniform and P2 = 1, then (11) is equivalent to the “proba-

bly approximately complete” measure of completeness pre-

sented in Das et al. (2011). Also note that this is a gen-

eralization of the authors’ previously proposed definition

of coverage proposed in Paull et al. (2014c) that required

that the expectation of the coverage be greater than some

new threshold. This definition can be achieved by setting

P2 = 0.5.

26 The International Journal of Robotics Research 37(1)

2.3. Propagating robot pose uncertainty to

coverage distribution

The distribution p(W̆ it = w) can be determined by propagat-

ing the uncertain cell location in the sensor frame through

the coverage sensor model, H:

W̆ it = H( sCi

t) (12)

Unlike most other works that assume the coverage sensor

function is a uniform mapping Acar et al. (e.g. 2006); Acar

and Choset (e.g. 2002b); Acar et al. (e.g. 2002: among oth-

ers) here we only require that the coverage sensor function

be piecewise continuous and differentiable.

The PDF of W̆ it can be found by mapping the cell loca-

tion RV, sCit, through the coverage sensor function H based

on the following (which is referred to as the “Fundamental

Theorem” in Papoulis and Pillai (2002)):

p(W̆ it = w)=

p( sCit = c[1])

|H′(c[1]) |+ · · · +

p( sCit = c[m])

|H′(c[m]) |(13)

where c[1], . . . , c[m] are the m solutions found when solv-

ing w = H(c):

w = H(c[1])= · · · = H(c[m]) (14)

and H′(c) is the derivative of H(c).

Equation (13) gives us an explicit way of evaluating the

coverage distribution of each coverage grid cell from a sin-

gle uncertain pose. Here H(c) is evaluated from the cover-

age sensor model (2) and p( scit) is generated by mapping

the cell location in the global frame through the uncertain

transformation sgT(xt) defined in (1).

2.4. Coverage with non-uniform sensor functions

We begin by presenting a second example with a non-

uniform coverage sensor function.

Example 2 (Lawnmower with uneven blades). A fictional

1D lawnmower whose blades are not parallel to the ground

is shown in Figure 6 (top). In this case, coverage is rep-

resented as the length of the grass cut by the blades. The

coverage sensor model, H is shown in Figure 6 (middle,

right) which maps the uncertain lawnmower pose (bottom

right) to the uncertain coverage distribution (middle, left)

through the application of (13).

2.5. Complete coverage guarantee

Using (13), we can now evaluate whether or not the cell

should be considered covered according to (10). The fol-

lowing lemma formalizes how this determination is made.

Lemma 1. Cell ci is covered according the definition (10)

by a single observation if∫

Ci

p( scit) dsci

t ≥ P2 (15)

where Ci is the interval of scit that satisfies H( sci

t)≥ wth

Fig. 6. The uncertain cell location, scit (bottom right), is propa-

gated through the coverage sensor model (middle right) to obtain

the coverage distribution resulting from only time t (middle left)

for the lawnmower with uneven blades (top). The generation of

one value in the coverage distribution is shown as the sum of the

probabilities of two possible cell locations (indicated by red and

green).

Proof. From (10) with a slight modification that a cell is

considered to be covered by a single observation if p(W̆ it ≥

wth)≥ P2. To prove the Lemma it is sufficient to show that:

p(W̆ it ≥ wth)=

∫

Ci

p( scit) dsci

t (16)

We can write the probability that cell ci is covered to a

value of w at time t based on the theory of total probability:

p(W̆ it = w)=

∫ ∞

−∞

p(W̆ it = w|sci

t) p( scit) dsci

t (17)

The probability that cell ci is considered covered by this

sensor swath according to (10) is the probability that W̆ it ≥

wth where wth is the threshold defining coverage defined in

(4):

p(W̆ it ≥ wth)=

∫ 1

wth

∫ ∞

−∞

p(W̆ it = w|sci

t) p( scit) dsci

t dw

(18)

which can be rewritten as

p(W̆ it ≥ wth)=

∫ ∞

−∞

p( scit)

∫ 1

wth

p(W̆ it = w|sci

t) dw dscit

(19)

There is a deterministic mapping from a given cell location

to a coverage value as defined by the coverage function H,

therefore the inner distribution is a singleton and can be

represented using the Dirac delta function

p(W̆ it = w|sci

t)= δ(w−H( scit) ) (20)

Paull et al. 27

Fig. 7. Coverage on one pass according to Lemma 1. The blue

areas represent the portion of distribution of the cell location in the

frame of the coverage sensor (shown in red) that is covered. The

dotted line is wth, the threshold for considering a cell covered. On

the left is the 1D vacuum cleaner example and on the right is the

uneven lawnmower. In the case of the uneven lawnmower, there is

a region of the distribution that represents that coverage is disjoint.

whose integral evaluates to either 1 or 0 depending on the

value of H( scit):

∫ 1

wth

δ(w−H( scit) ) dw =

{

1 if H( scit) )≥ wth

0 otherwise(21)

Now define the set Ci to be the interval of scit for which

H( scit) )≥ wth holds, then the result is that

p(W̆ it ≥ wth)=

∫

Ci

p( scit) dsci

t (22)

and the result is proven.

A visualization of Lemma 1 is shown in Figure 7.

Lemma 1 provides an explicit method of evaluating

whether a cell is “probabilistically” covered, however, we

only care about whether the cell is actually covered. In

the following theorem we show the conditions under which

probabilistic coverage implies actual coverage.

Theorem 1. Let 8N ( r) be the probability that a point sits

at a Mahalanobis distance of less than or equal to r, and sc̃it

be the actual location of cell i in the sensor frame at time t,

and assume that scit ∼ N ( µi

t, 6t).

If cell ci is covered according to (10) and the Maha-

lanobis distance between the actual sensor pose and the

estimated sensor pose is bounded by α

‖sc̃it − µi

t‖6t ≤ α (23)

with

α = 8−1N ( P2) (24)

then the cell ci was actually covered according (4) as long

as the set Ci is convex and P2 ≥ 0.5.

Proof. Recall from Lemma 1 that a cell is covered accord-

ing to (10) if∫

Ci

p( scit) dsci

t ≥ P2 (25)

where Ci is the interval under which H( scit)≥ wth.

Define C̄i as the opposite interval (H( scit) < wth). The

minimum Mahalanobis distance, rmin, to the border of the

coverage interval is given by

rmin = minc∈C̄i‖c− µi

t‖6t (26)

Fig. 8. Condition for guaranteed coverage according to Theorem

1. Left: A 1D example. Pink rectangle denotes coverage sensor.

Red arrow denotes actual cell location in coverage sensor frame.

Green arrow denotes mean of coverage cell location distribution.

Since the Mahalonobis distance from µ to c̃ is less than the Maha-

lanobis distance from µ to cmin, this cell is guaranteed to be

actually covered. Right: Extension to a more realistic 2D scenario

(all colors have equivalent meaning).

and denote the corresponding location to be cmin. Then we

can say that the cell is actually covered (according to (4)) if

‖cmin − µit‖6t > ‖sc̃i

t − µit‖6t (27)

which is equivalent to saying that r̃ < rmin where r̃ is the

Mahalanobis distance of the actual location of the cell from

the mean estimate.

If we create another interval C̃i that is the constant

Mahalanobis distance interval

C̃i = {c s.t. ‖c− µi

t‖6t < rmin} (28)

then we know that∫

C̃i

p( scit) dsci

t ≤

∫

Ci

p( scit) dsci

t (29)

and

8N ( rmin)=

∫

C̃i

p( scit) dsci

t (30)

Thus, if 8N ( rmin)≥ P2, then the cell is also covered accord-

ing to (10) (this is a less-stringent requirement than Lemma

1). The convexity of Ci implies that if the mean of the cell

location µic sits outside of the set H( µ

ic)≥ wth, then the

value of the integral in (22) is upper bounded by 0.5 and

since P2 ≥ 0.5, then this cell cannot be covered.

The cumulative density function 8N is monotonically

increasing, so we can invert rmin ≥ 8−1N ( P2) and, consid-

ering the original statement of the theorem (23), then we

can write5

r̃ ≤ 8−1N ( P2)≤ rmin (31)

therefore r̃ ≤ rmin and the cell is actually covered according

to (4).

Consequently, a plan that guarantees complete coverage

with respect to the probabilistic criterion and also satisfies

the conditions of Theorem 1 is guaranteed to achieve full

28 The International Journal of Robotics Research 37(1)

coverage in the deterministic sense. Adaptive planning to

achieve complete coverage with respect to (11) is discussed

in Section 5.

It should be noted that the consistency condition (23) is

not actually enforceable in reality since it is not observable

(we do not know where the robot actually is). In reality, this

condition has a probability of being met that we evaluate.

In effect, we now have a way to be arbitrarily confident

that complete coverage is achieved without planning for the

worst-case absolute error condition.

2.6. Generating the coverage distribution

recursively from subsequent measurements

of the same location

Equation (13) provides a method for propagating the pose

uncertainty through the sensor characteristic to obtain an

estimate of the coverage distribution from a single sensor

pose. Over the course of a coverage mission, the robot will

move around the workspace and may “cover” the same loca-

tion many times. We require a way of combining the cov-

erage distribution from time t with the previous coverage

distribution.

The most conservative way of fusing measurements of

the same location at different times is by assuming that

they are statistically dependent, in which case we apply

the “MAX” function (the coverage can be no less than

the greatest value it was covered to in all of the previous

passes; clearly this is the appropriate assumption for the

grass cutting example).

Using this statistical dependence assumption, we recur-

sively define a lower bound on the coverage distribution at

time t as a function of the coverage distribution at time t−1

and the coverage resulting from only time t as given by the

following:

W it ≥ max(W̆ i

t , W it−1) (32)

where the distribution of W it can be evaluated through the

following relation (Papoulis and Pillai, 2002):

p(W it = w)=p(W̆ i

t = w)

∫ w

0

p(W it−1 = ε) dε

+ p(W it−1 = w)

∫ w

0

p(W̆ it = ε) dε

(33)

Based on this MAX function formulation for combining

coverage sensor measurements for a given cell ci, we have

a way of updating the coverage distribution at time t based

only on the pose at time t (used to generate W̆ it ) and the

coverage distribution at time t − 1, W it−1, meaning that the

Markov assumption can be applied to the coverage sensor

measurements and pose information can be discarded once

the coverage distribution at time t has been updated. The

algorithm for iterative coverage distribution estimation is

summarized in Algorithm 1.

Example 3 (Lawnmower with uneven blades (continued)).

Two poses from different times cut the same piece of grass

Algorithm 1 iterative_coverage_update

Input: Current pose belief bel(xt−1)

Probabilistic coverage map W1,...,nt−1

Odometry ut and measurement zt

Location of coverage map grid cells c1,...,n

Output: Updated coverage map W1,...,nt

1: ¯bel(xt)=∫

p(xt|xt−1, ut) bel(xt−1) dxt−1

2: bel(xt)= ηp( zt|xt) ¯bel(xt)

3: for all i such that ci ∈ S ′t do

4: p( scit)= p( s

gT( Xt)gci

t = c)

5: W̆ it = H( sci

t)

6: W it = max(W̆ i

t , W it−1)

7: end for

as shown in Figure 9. The lawnmower pose distributions

are shown in row (a), the sensor function is shown in row

(b), the coverage distributions from the individual passes

are shown in row (c), and finally the combined coverage

distribution is shown in row (d) through the application

of (33).

3. Full trajectory estimation for coverage

The previous section provides an explicit method of

accounting for pose uncertainty in the coverage model. The

iterative algorithm (Algorithm 1) admits any state estima-

tion method that approximates the optimal Bayes’ filter.

However, coverage over the workspace is dependent on the

entire sensor trajectory, therefore to obtain the best esti-

mate of the coverage belief using the proposed framework,

we require the best estimate of the entire vehicle trajec-

tory as opposed to just the present state: smoothing rather

than filtering. This follows recent trends in robotics infer-

ence where, in many cases, the smoothing problem can

be solved efficiently and incrementally, in real-time, by

exploiting the underlying sparsity of the information matrix

(Kaess et al., 2008). This approach is particularly appli-

cable to the case when a vehicle is receiving intermittent

measurement updates as can be the case in underwater sce-

narios, sparsely communicating networks, or even intermit-

tent loop closures in simultaneous localization and mapping

(SLAM).

The same seabed coverage mission shown in Figure 1

where vehicle estimation is done with an EKF was per-

formed with a non-linear least-squares smoother (iSAM

(Kaess et al., 2008)) to estimate the entire AUV trajectory

and the results are shown in Figure 10. Once again, the vehi-

cle dead reckons while inside the environment, and then

obtains GPS updates at the end of each track. In this case,

the area that we believe we have covered (Figure 10(b)) and

the area that we have actually covered (Figure 10-c) have

a much closer match than in Figure 1. Figure 10(d) shows

the mean probability of coverage over the workspace deter-

mined using the probabilistic framework presented in the

previous section.

Paull et al. 29

Fig. 9. Cell ci is covered twice by the uneven lawnmower. In each

case, the distribution of the cell location in the sensor frame (row

(a)) is mapped through the coverage sensor model (row (b)) to

obtain an instantaneous coverage distribution (row (c)). These two

distributions are combined using the max operation on RVs to

obtain the final distribution (row (d)) that represents the coverage

distribution for cell ci resulting from both coverage readings.

Proposition 1. Reduction of the covariance of state esti-

mates increases the area covered in the probabilistic frame-

work if the following conditions are met:

1. Cell location distributions are Gaussian ( scit ∼ N

( µic, 6c) ) where the covariance is fixed for all cells in

the sensor swath),

2. Coverage is defined by (11) with P2 ≥ 0.5,

3. The interval Ci is convex.

In this case the number of cells covered is a monotonically

decreasing function of the covariance 6c.

This is a direct result of Lemma 1 and Theorem 1.

The value of the integral in (22) will decrease monoton-

ically with the covariance as a result of the monotonicity

of the Gaussian cumulative distribution function (CDF).

Since the value of the integral decreases monotonically with

the covariance, then the number of cells that satisfy the

condition of (22) will also decrease monotonically.

Fig. 10. (a) Plot of environment to be covered (yellow) with

desired tracks (magenta), estimated path (red), and actual path

(blue). (b) Estimated coverage. (c) Actual coverage. (d) Mean

probability of coverage.

Fig. 11. The area covered is the number of cells that have an area

of their distribution of the cell location in between the points c1

and c2 greater than P2. The number of cells covered decreases as

the covariance 6c increases.

For visualization purposes, consider the case of a uni-

form 1D coverage function over an interval c1–c2 and zero

outside (a 1D version of the vacuum cleaner). In this case

the set Ci is equivalent to the closed interval [c1, c2], as

shown in Figure 11. If we consider slices of the mesh for

constant values of 6c we can see that more areas will be

covered as 6c → 0.

Since the minimal state uncertainty will, in general, pro-

vide the most efficient completion of the coverage task,

if we obtain some information at time t that allows us to

improve our estimate of the robot pose at some previous

time t′ < t, then we can increase the total effective area

of the workspace that has been covered by recalculating the

coverage based on the updated pose estimate. In general,

this will be done as a batch operation where the coverage

grid map is reset and then all sensor poses are reprocessed

from t′ = 1 → t. As a result we wish to obtain the max-

imum a posteriori (MAP) estimate of the entire robot tra-

jectory, bel(x1:t)= p(x1:t|z1:t, u1:t,x0) as opposed to just the

30 The International Journal of Robotics Research 37(1)

Fig. 12. Factor graph representation of the MAP estimation prob-

lem. White circles indicate hidden states to be estimated. Small

colored states represent measurements that constrain the hid-

den states. Blue, prior measurement; yellow, odometry; green,

intermittent position (GPS) update.

current state bel(xt) (Dellaert and Kaess, 2006):

x̂1:t = argmaxx1:t

p(x1:t, u1:t, z1:t)

= argminx1:t

− log bel(x1:t)(34)

If we assume that the process model f (xt, ut) and mea-

surement model h(xt) are corrupted by additive Gaussian

noise with covariance Qt and Rt, respectively, then the

MAP estimation (34) can be reformulated as a non-linear

least-squares problem:

x̂1:t =

t∑

t′=1

‖ f (xt′−1, ut′ )−xt′‖2Qt′

+

t∑

t′=1

‖h(xt′ )−zt′‖2Rt′

+ ‖x0 − x̂0‖260

(35)

where x̂0 and 60 are the mean and covariance of the prior,

respectively, and we have used the standard squared Maha-

lanobis distance notation ‖e‖26 = e6−1eT. This estima-

tion model can be represented as a bipartite factor graph

(Dellaert and Kaess, 2006) as shown in Figure 12, where

the large white circles represent the states to estimate,

and the small colored circles represent different forms of

measurements.

The MAP estimate is the best estimate of the entire tra-

jectory based on all past measurements. However, since

the coverage is determined by the entire trajectory, every

time a batch adjustment is performed to the trajectory, the

entire coverage map must be re-computed from scratch.

As missions become longer, this will become computation-

ally intractable. To compensate, an adaptive sliding window

filter approach is proposed.

3.1. Adaptive sliding window filter for coverage

state estimation

The proposed approach is closely related to that of Sibley

et al. (2010) where a sliding window approach is used for

simulating a planetary landing operation. However, in Sib-

ley et al. (2010), the size of the window used is fixed. Here,

since we assume no prior knowledge of the time interval of

Fig. 13. New factor graph after applying the adaptive sliding

window criterion.

intermittent updates or their quality, we propose an adaptive

sliding window sized based on an information-theoretic cri-

terion. As a result, we can generate a good estimate of the

trajectory without unbounded growth in computation over

time.

We maintain only poses in the state space that have

“appreciable” entropic information gain H resulting from

an intermittent update at time t, Zt:

tsw = argmint′∈{1,...,t}

t′

s.t. H( Xt′ | U1,...,t, Z1,...,t−1)−H( Xt′ | U1,...,t, Z1,...,t) > ε2

(36)

where ε2 is a tunable threshold that will affect the adap-

tive window size. In the Gaussian case, we can rewrite the

constraint in (36) as

1

2( |6t′|t−1| − |6t′|t|) > ε2 (37)

where we have used the shorthand notation t′|t − 1 to rep-

resent the covariance at time t′ after having incorporated all

measurements up to and including time t − 1.

After performing the sliding window criterion (36), all

states from times before tsw can be marginalized out with

Xtsw becoming the new first pose which is constrained by

a unary measurement X̂tsw the MAP estimate of the pose

at the sliding window time before marginalization with

associated covariance as determined through Schur com-

plement operation on the information matrix (Huang et al.,

2013). The resulting factor graph is shown in Figure 13 and

the non-linear least-squares formulation is updated from

(35) to

x̂tsw:t =

t∑

t′=tsw+1

‖ f (xt′−1, ut′ )−xt′‖2Qt′

+

t∑

t′=tsw+1

‖h(xt′ )−zt′‖2Rt′

+ ‖xsw − x̂sw‖26sw

(38)

The coverage map is stored at the new sliding window

time and on subsequent updates is only recomputed from

this sliding window time forwards. The values of x̂0,...,tsw−1

can be discarded as they are no longer needed (all of the

useful pose information is stored in the coverage map).

Paull et al. 31

Algorithm 2 Sliding Window Coverage Estimation

1: p(W i0 = w0)= 1 ∀i = 1, . . . , n

2: bel( x0)← distribution of prior pose state estimate

3: t← 1

4: tsw ← 0

5: Store W 1:ntsw

6: repeat

7: Calculate |6t′|t−1| ∀t′ = tsw → t − 1

8: x̂tsw:t = argmaxxtsw :t

p(xtsw:t, utsw:t, ztsw:t)

9: if New global measurement, zt then

10: load W 1:ntsw

11: for t′ = tsw + 1 : t do

12: Recover marginal covariance 6t′ (e.g. using the method described in Kaess and Dellaert (2009))

13: bel( xt′ )= N ( x̂t′ , 6t′ )

14: W 1:nt′+1← iterative_coverage_update( bel(xt′ ) , W 1:n

t′, ut′ , zt′ )

15: end for

16: Calculate |6t′|t| ∀t′ = tsw → t

17: tsw ← argmint′∈{tsw,...,t}

t s.t. (37) is satisfied

18: Marginalize out all states older than tsw

19: Store W i:ntsw

20: else

21: W 1:nt+1 ← iterative_coverage_update( bel(xt) , W 1:n

t , ut)

22: end if

23: t← t + 1

24: until Mission completion

The full algorithm for coverage estimation based on the

sliding window approach is summarized in Algorithm 2.

Lines 1–5 constitute the initialization before entering the

mission loop in line 6. The prior entropy is calculated in

line 7 and the new trajectory estimate is obtained in line 8.

If a new intermittent update is received, then the coverage

map is recalculated starting from the sliding window time

tsw in lines 11–15 by iterating from the sliding window time

to the current time t and performing the iterative coverage

at each timestep. Once complete, the posterior entropy is

calculated in line 16 and used to generate the new sliding

window time. The coverage map at the new tsw is stored for

later use (line 17) and older poses are marginalized away

(line 18). In the case that no global update was received, the

entire trajectory would not be significantly altered so only

an iterative update similar to what is described in Algorithm

1 is performed.

4. Robust cooperative trajectory estimation

The area coverage problem is an excellent candidate for

distributed systems as it is inherently decomposable. If the

area to be covered can be partitioned into N distinct sub-

areas, then a team of N robots should be able to cover

the entire area N times faster. Here, we assume that this

is done and show that the benefits are even more signifi-

cant considering the probabilistic coverage objective in the

case that the robots can communicate and make relative

measurements of each other, a scenario usually referred to

as cooperative localization (Roumeliotis and Bekey, 2002).

However, in this case, we wish to update the entire robot

trajectories based on communicated information and rel-

ative measurements. A fully distributed algorithm is pre-

sented in Nerurkar et al. (2009), but requires very high

bandwidth and reliable communications interface. In many

cases, such as space, underwater, or in disaster recovery

scenarios, such a communications link is not available.

Here we present a cooperative trajectory estimation that

is robust to low-bandwidth and unreliable communication

links. We make no assumptions about the communications

bandwidth, throughput, reliability, or topology.

We begin by considering the centralized case that

exploits relative measurements and then we propose a

communication-reduced and robust decentralized trajectory

estimation alternative.

4.1. Centralized cooperative trajectory

estimation

The centralized state consists of all robot poses for all time:

xc , x1,...,N0,...,t . We extend the single-vehicle non-linear least-

squares formulation from Section 3 to include other vehicle

poses. Again, we assume that robots are estimating their

poses through odometry and occasional global updates,

except now we also include relative measurements between

32 The International Journal of Robotics Research 37(1)

Fig. 14. Factor graph representation of multi-robot cooperative

trajectory estimation between two robots. Each robot estimates its

own position through odometry, u, and occasional global updates

g. Vehicles can additionally improve their pose estimates through

relative observations r.

robots ri,j

t′k

that occur between robot i and robot j at times

tk′ , k′ = 1, . . . , k:

x̂c = argminxc

{

t∑

t′=1

N∑

i=1

1

2‖ f (xi

t′−1, uit′ )−xi

t′‖2

Qit′

+

t∑

t′=1

N∑

i=1

1

2‖h(xi

t′ )−zit‖

2

Rit′

+

N∑

i=1

1

2‖xi

0 − x̂i0‖

2

6i0

×

K∑

k=1

N∑

i=1

N∑

j=1i6=j

1

2‖h(xi

tik

, xj

tjk

)−ri,jtk‖2

4i,jk

}

(39)

where the h(xi

tik

, xj

tjk

) is the relative measurement model,

and the relative measurements are assumed to be corrupted

by additive Gaussian noise with covariance 4i,jk . The fac-

tor graph representation of (39) is shown in Figure 14.

The problem for unreliable communication networks is that

each vehicle requires knowledge of all proprioceptive and

exteroceptive measurement data locally collected by all

vehicles in order to solve (39). In the case that there are

communication failures, this data will start to backlog.

4.2. Decentralized cooperative trajectory

estimation

Here we propose a modified version of (39) where the

amount of data required to be passed between vehicles is

dramatically reduced and robust to communication failures.

The key to the approach is that each robot only needs to esti-

mate the poses of others at communication/measurement

times to obtain all of the benefits of cooperative trajectory

estimation locally.

Each vehicle j locally maintains two factor graphs as

shown in Figure 15. The first consists of own-robot poses

Fig. 15. Factor graph representation of decentralized multi-robot

trajectory estimation. Each robot now maintains two factor graphs.

Top: The new local multi-robot factor graph. Bottom: The DR

pose graph. Marginalization is performed on the factor graph to

compute the factors that other members of the team require in

order to generate their own local multi-robot trajectory estimates.

for all time and other vehicle positions for all communica-

tions/measurement times:

xj

d , [x1:j−11:k , x

j

1:t, xj+1:N1:k ]t (40)

The second is called the DR factor graph and is used to

generate the factors that will be transmitted to other vehi-

cles. From the DR factor graph we can generate a change

in pose factor (estimate and associated covariance) from

any start time to any end time by marginalizing out inter-

mediate position nodes. As a result, each robot need only

maintain one DR pose graph since it can be used to generate

the required factors for transmission to all other vehicles. In

this case, marginalization is equivalent to composition. For

example, to combine position factors from time t1 to t2:6

xjt2=x

jt1⊕1x

jt1→t2+ ζ

jt1→t2

(41)

where

1xjt1→t2=

t2∑

t=t1+1

xj

t−1 ⊕xjt (42)

and

ζjt1→t2

,

t2∑

t=t1+1

ζj

t−1,t ∼ N ( 0, Qjt1→t2

) (43)

with

ζj

t−1,t ∼ N ( 0, Qj

t−1,t)

Qj

t−1,t = Jt−1⊕Qt−1JTt−1⊕ + Jt⊕QtJ

Tt⊕

(44)

where Jt−1⊕ and Jt⊕ are the Jacobians of the composition

operator ⊕ with respect to each of the individual measure-

ments being composed (Smith and Cheeseman, 1986). This

formulation (42)–(44) represents a generative process by

Paull et al. 33

which a vehicle can locally generate the desired factors

for transmission to other vehicles based on the following

bookkeeping scheme.

Bookkeeping Bookkeeping is required for vehicles to

know which local factors should be generated to guaran-

tee consistency of the multi-vehicle estimates maintained

by others. Each vehicle i maintains a set of N − 1 incoming

(C iin) and outgoing (Ci

out) confirmed contact points. These

contact points are the times of most recent confirmed suc-

cessful communications to and from each other vehicle in

the team.

Incoming contact points are easily detectable based on

the times at which communications are received. Outgo-

ing contact points necessitate the use of communicated

acknowledgment bits that are sent in subsequent data packet

transmissions. In the case that an acknowledgment com-

munication also fails, the contact point time will not be

updated, in essence assuming that the previous outgoing

communication had failed. However, in the case that this

implied assumption is incorrect, the receiving vehicle will

still be able to recover the appropriate factor from the

data sent using the subtraction property for change in pose

factors (see Section 4.2.1).

Packet transmission Consider the case where vehicle i

makes a transmission at time tK . The following data should

be included in the data packet:

• the change in pose factors from incoming and outgoing

contact point times to the present time;

• relative measurement data associated with each of the

incoming contact point times;

• a local global measurement if one has been made since

the oldest contact point and a change in pose to the time

of the global measurement, tg;

• a set of N − 1 acknowledgment bits.

4.2.1. Packet reception. Upon reception of a packet on

vehicle j sent from vehicle i, the receiver must generate the

correct factors to compute the MAP estimate of xj

d . Gen-

erating the correct change in pose factors that relate the

positions of other vehicles to own-vehicle poses can pos-

sibly require a subtraction operation to be performed on the

change in pose factor. For example, consider the case where

robot j receives two changes in pose factors from robot i at

time t, 1xit1→t and 1xi

t2→t with t1 < t2. Then 1xit1→t2

can

be recovered using

1xjt1→t2= 1x

jt2→t ⊕1x

jt1→t

Qt1→t2 = J−1t2⊕

( Qt1→tJt1⊕Qt2→tJt1⊕) J−Tt2⊕

(45)

This is a valid operation since the factors are built using

simple composition in (42) and (44). For a visual depic-

tion refer to Figure 16. This is required when a previous

transmission that was assumed to have failed was actually

Fig. 16. The subtraction property illustrated on the changes of

pose factors. The top two changes in pose factors are received and

are decomposed into the single bottom position chain.

successful. The key advantage is that robot j can recover

the appropriate data needed for its own local multi-vehicle

factor graph. The multi-vehicle factor graph is guaranteed

to remain connected and consistent at all times because

the changes in pose factors originate from times of known

communication.

4.2.2. Centralized→ decentralized. To obtain a decentral-

ized multi-vehicle trajectory estimate, each robot, j, locally

solves the following non-linear least-squares problem:

xj

d

∗= argmin

xjd

{

t∑

t′=1

1

2‖ f (x

j

t−1, ujt)−x

jt‖

2

Qjt

+

k∑

k1=1

k∑

k2=k1

∑

i=1,...,Ni6=j

1

2‖xi

t′k1

⊕1xit′k1→t′

k2

− xit′k2

‖2

Qit′k1→t′

k2

+

t∑

t′=1

N∑

i=1

1

2‖h(xi

t′ )−zit‖

2

Rit′

+

N∑

i=1

1

2‖xi

0 − x̂i0‖

2

6i0

×

k∑

k′=1

N∑

i1=1

N∑

i2=1i2 6=i1

1

2‖h(x

i1k′

, xi2k′

)−ri1,i2tk′‖2

4i1,i2tk′

}

(46)

which is identical to (39) except that the odometry factors

have been re-organized into own-vehicle odometry (first

term) and other-vehicle changes in pose (second term). The

factor graph representation is shown in Figure 15 (top).

4.2.3. Data throughput required for decentralized trajec-

tory estimate. Data to be transmitted is at most:

• 2( N−1) changes in pose factors (comprising value and

associated covariance);

• N − 1 relative measurements;

34 The International Journal of Robotics Research 37(1)

Fig. 17. Finally we can apply the sliding window constraint to

maintain constant-time scalability. The new prior, x̂1,2tsw

, operates

over all robot poses at the new sliding window time tsw.

• one global factor with associated change in pose;

• N − 1 acknowledgment bits.

Scaling is linear with respect to the size of the robot team

N and constant with respect to time t even in the worst case

of communications dropouts.

4.3. Cooperative trajectory estimation with

adaptive sliding window

We follow the same procedure as described in Section 3 for

applying the sliding window filter since, as before, very old

states will cease to be appreciably updated as time passes.

We add the extra constraint into (36) that the sliding win-

dow time should also not be greater than the smallest con-

tact point time. There is one additional complication here,

namely that the marginalization of old states is done over

the old states in the multi-vehicle factor graph, and there-

fore the new prior operates over all vehicles in the team as

shown in Figure 17.

5. Adaptive planning

The proposed probabilistic coverage framework admits a

new class of adaptive planners over the coverage map. Any

structured coverage path planner that requires the selec-

tion of a spacing parameter based on the size of the cov-

erage implement can be used adaptively. Here, we use a

Boustrophedon-style approach (Choset, 2000) and dynam-

ically track the spacing value based on the most up-to-date

estimate of the coverage map and the robot pose sensor

uncertainty. In this case, the planner will automatically

adapt to conditions of better localization accuracy to cover

the uncovered areas more aggressively. As an example,

consider Figure 18.

The adaptive planning strategy has two main

components.

Fig. 18. The optimal value of d is calculated such that the vehicle

achieves maximal coverage while still guaranteeing full coverage

in the survey direction. A feedback controller is used to update the

transect location in real-time to track the desired value of d.

1. Calculate the value of d to maximize coverage while

guaranteeing complete coverage in the survey direc-

tion (based on the probabilistic coverage criterion (11)).

When vehicle position uncertainty is high, the tracks

will need to be spaced more closely to guarantee cov-

erage. Upon reception of new localization information,

the entire vehicle trajectory is updated with reduced

uncertainty, and consequently the value of d, which cor-

responds to the spacing of the current track, can be

increased resulting in faster coverage.

2. Design a feedback controller to track the value of d. If

we consider the path tracking error to be bounded,

‖xdt − x̂t‖2 < η (47)

where xdt is the desired robot location (on the path)

and x̂t is the estimated robot position (not actual). We

compensate for this error by reducing the value of d

by η.

If Theorem 1 and (47) hold, then coverage will be achieved

in the real-world sense, if possible. In addition, robots that

can self-localize more accurately will automatically achieve

the coverage task more quickly, as will be shown in the case

of robots performing coverage and cooperatively localizing

in Section 7. Note that this does not require any additional

tuning or changing of parameters.

6. Adaptive and robust cooperative AUV

seabed coverage

The framework presented in Section 2–5 is particularly

well-suited to the task of AUV seabed surveying for several

reasons.

1. Localization error cannot be considered as negligible

in most AUV seabed surveys since the AUV has no

Paull et al. 35

access to a global position reference when submerged.

In addition, the platform uncertainty is not fixed or even

necessarily bounded as in Bosse et al. (2007).

2. The mission is safety-critical, and thus requires stronger

guarantees about the quality of the coverage than can be

offered by previous methods.

3. Underwater vehicles are able to communicate through

a faulty and bandwidth-reduced channel. In addition,

vehicles can make relative measurements of one another

through time-of-flight (TOF) of the signals, making this

an ideal case for the cooperative trajectory estimation

framework described in Section 4.

4. A submerged AUV’s ability to navigate is usually

directly related to the cost of the on-board sensors.

Therefore, if we provide coverage guarantees regard-

less of sensor quality, then we can potentially drastically

reduce the cost of performing these operations (at the

expense of mission time).

6.1. Coverage sensor model

Many underwater seabed surveying missions are conducted

with a SSS sensor. The SSS uses the returns from emitted

high-frequency sound to generate an image of the seabed.

An object sitting on the seabed will cast a sonar shadow that

can be analyzed to determine whether the shape is sugges-

tive of a mine. The on-board SSS gathers data as the AUV

moves forward in rectilinear motion as shown in Figure 19

(top).

In 2006, there was an effort by the NATO Undersea

Research Centre to build a model to quantify mine-hunting

performance with respect to all factors that can influ-

ence the probability of detecting a target in sonar data.

The result was the Extensible Performance and Evaluation

Suite for Sonar (ESPRESSO) (Davies and Signell, 2006).

ESPRESSO generates a lateral range curve for a given set

of parameters (conditions), which is used to model the cov-

erage performance of the sonar. Figure 19-bottom shows

the lateral range curves generated by ESPRESSO for three

different seabed types: cobble, sand, and clay, all at a depth

of 10 m. The “confidence” at a lateral range y represents

the probability that if a mine is present it will be detected.

As can be seen from Figure 19, these lateral range curves

are non-linear and non-uniform, making the coverage task

more complex. Note that since this is a binary classification

problem, the minimum value of the P( y) is 0.5 and this is

now the minimum coverage value.

6.2. AUV Dead Reckoning

We assume the following sensor/communications suite for

the AUV.

• A DVL sensor that measures the speed of the robot rela-

tive to the stationary seabed (assuming that the altitude

is below the maximum range of the sensor).

• A three-axis compass for globally referenced attitude.

Fig. 19. Top: The AUV moves through the water to generate an

image of the seabed. Bottom: Based on the environmental con-

ditions, ESPRESSO can generate a lateral range curve P( y) that

represents the “confidence” that if a mine exists at lateral range y,

that it will be detected correctly. Three sample lateral range curves

are shown here for three different seabed types.

• A GPS receiver for global position updates that only

works when the vehicle is at the surface.

• An acoustic modem with an on-board oscillator that

can maintain a precise pulse-per-second (PPS) refer-

ence signal that is synchronized between all vehicles

(more details are given in Section 6.4).

• A SSS for mapping only (not used for localization).

When the AUV is submerged it “dead reckons” using

velocity estimates from a DVL and orientation estimates

from a three-axis compass.

At some interval, such as when the robot surfaces for a

GPS fix or receives an acoustic localization update, the full

trajectory is re-estimated to obtain a better estimate of the

coverage.

6.3. The probabilistic coverage map

Since the SSS can only generate useful mosaicked data

when the AUV is in rectilinear motion, we assume that only

the cross-track uncertainty in the AUV location is consid-

ered as it will have a larger effect on the coverage uncer-

tainty in the case the AUV is moving in a straight line. In

addition, since the data from the SSS during turns is unus-

able, the vehicle will have to exit the workspace in order to

turn and make a subsequent pass.

36 The International Journal of Robotics Research 37(1)

Fig. 20. Projection of 2D position distribution onto the 1D line

orthogonal to AUV motion.

As a result, the pose distribution is projected onto the

line orthogonal to the direction of AUV motion as shown in

Figure 20.

The cell location in the global frame is transformed

through sgT( Xt) into a distribution in the sensor frame

p( sCit = c) and then the along-track (sX i

t ) uncertainty is

marginalized out to yield a distribution for cell i in the

cross-track direction, p( sY it =

syit)∼ N ( µsyi

t, σsyi

tsyi

t) with

µsyit=( xi − µxt ) sin θt+( yi − µyt ) cos θt (48)

and

σsyitsyi

t= σ 2

xtxtsin2 θt+ρσxtxtσytyt sin 2θt+σ 2

ytytcos2 θt (49)

where ρ is the bivariate correlation coefficient between Xt

and Yt.

The coverage sensor model, H, originally defined in (2),

is now given by the lateral range characteristic and oper-

ates over the distribution of the cell location in the lateral

direction:

W̆ it = H( sCi

t)= P( sY it ) (50)

Figure 21 shows one instance of generating the distribu-

tion of W̆ it by mapping an uncertain cell location through

the sensor characteristic. The distribution of the orthogonal

distance of the cell from the sensor is shown in red (bot-

tom right). The sonar coverage sensor model is shown in

blue (top right). The cell location distribution is mapped

through the coverage sensor model to obtain the coverage

distribution resulting from this one single measurement (top

left).

We have the machinery in place now to execute the

single-vehicle sliding window coverage estimation algo-

rithm (Algorithm 2). When the vehicle is submerged, the

iterative algorithm is executed. Upon surfacing, the slid-

ing window criterion is executed and the coverage map is

recomputed from the previous sliding window time.

6.4. AUV cooperative trajectory estimation

Underwater communications over any appreciable dis-

tance is restricted to the acoustic channel. Communicating

through acoustics has several challenges (Webster et al.,

Fig. 21. The distribution of the cell in the sensor frame (bottom

right) is mapped through the P( y) curve (top right) to obtain a

coverage distribution (top left).

Fig. 22. Concept showing acoustic communications amongst

three AUVs in a TDMA scheme. The green AUV transmits at time

t1, followed by the blue AUV at time t2, and finally the red AUV

at time t3. Each reception enables the receiver to obtain a relative

range measurement of the sender based on the travel time of the

packet and reduce its location uncertainty in the direction of the

sender (gray ellipse to black ellipse).

2012): high latency (packets travel at the speed of sound),

reduced bandwidth (nodes share the channel in a time divi-

sion multiple access (TDMA) scheme), and low reliability.

However, one potential benefit of communicating through a

high-latency channel is that packet receptions also yield a

relative range measurement through calculation of the TOF

of the packet (assuming known send time and synchronized

clocks (Webster et al., 2012)), which reduces the location

estimate at the receiver, as shown in Figure 22.

AUV j sends acoustic transmission k = 1, . . . , K at time

tk , tj

k and it is received on vehicle i at time tk +1i,jk , ti

k

where 1i,jk is the TOF of the acoustic packet. The result-

ing range measurement is represented as ri,jtk

. It should be

noted that, in reality, the acoustic transmission is sent from

point to point in 3D space. We project the range onto the 2D

Paull et al. 37

plane which requires knowledge of both vehicles’ depths, di

and dj:

ri,jtk

, r2D =( r23D−( di − dj)

2 )12 (51)

The range measurement model is given by

ri,jtk= h(xi

tik

, xj

tjk

)+δi,jtk

, δi,jtk∼ N ( 0, 42

tk)

= ‖[xi

tik

, yi

tik

]T − [xj

tjk

, yj

tjk

]T‖2 + δi,jtk

(52)

where 42rr is the covariance of the range measurement and

is assumed to be constant with time and independent of

range, a claim experimentally validated in Webster et al.

(2012). Subsequently, the decentralized trajectory estima-

tion scheme is applied with the range-only measurements

inserted into the MAP formulation (38).

7. Experimental results

We have tested and demonstrated the individual compo-

nents and the fully integrated system over a number of

years. In this section, we first describe the software frame-

work, and then present results for single-vehicle adap-

tive coverage, localization performance of our bandwidth-

constrained cooperative trajectory estimation scheme, and

then finally the implementation and results of our fully

integrated system.

7.1. Software architecture

The system is implemented with a combination of

open-source projects: the mission-oriented operating

suite with interval programming (MOOS-IvP (Benjamin

et al., 2009)), Goby–Acomms (Schneider and Schmidt,

2013), lightweight communications and marshaling (LCM)

(Huang et al., 2010), and iSAM (Kaess et al., 2008).

MOOS-IvP is a middleware and marine simulation soft-

ware framework. A single MOOS community resides on

each vehicle and a third as a shoreside for monitoring.7.

In Goby, acoustic packets are defined as Google Proto-

col Buffers that are encoded using the dynamic compact

control language at runtime. Received packets are parsed

in MOOS and then passed through LCM channels to the

back-end iSAM solver. This back-end solver manages the

multi-vehicle factor graph and uses built-in and custom fac-

tor definitions to maintain estimates of the entire vehicle

trajectory and feature locations. Upon receipt of a packet

request from MOOS, a relative factor-graph is built, opti-

mized, and the packet contents are sent back to MOOS for

transmission.

In all experiments, the coverage probabilistic coverage

criterion (11) was used with P2 = 0.5 and P1 = 0.99.

7.2. Cooperative trajectory estimation

We have also independently tested the performance of our

cooperative trajectory estimation scheme (for full details

Fig. 23. Vehicle estimate uncertainty growth for different packet

loss rates. Dashed lines indicate MAP (smoothed) estimates

at t = 500 s.

Fig. 24. Vehicle estimate uncertainty growth when one vehicle

receives intermittent global position (GPS) updates. The dashed

lines are the smoothed trajectory estimates computed at time

t = 500 s.

see Paull et al., 2014b). Figure 23 shows the trace of the

covariance of the positional uncertainty on-board one of the

vehicles while DR in a controlled situation where all pack-

ets were received. We randomly discard different percent-

ages of packets to analyze the robustness of the trajectory

estimation scheme to packet loss in transmission. In the

smoothing case, we can see that the estimates are almost

indistinguishable even for a 50% packet loss scenario. We

can also clearly see the advantage of smoothing for uncer-

tainty reduction over the entire trajectory, which results in

faster coverage.

Figure 24 shows an even more dramatic difference in the

case where one vehicle is able to bound its localization error

(for example, by obtaining a GPS fix). In addition, the ben-

efit of the global update is transferred to the rest of the team

through cooperative localization.

38 The International Journal of Robotics Research 37(1)

Fig. 25. The autonomous kayaks used for the experiments on the

Charles River in Boston, MA, USA. Each kayak tows an acoustic

transducer (shown on deck) to perform acoustic communications

and one-way travel time ranging.

7.3. Cooperative coverage with adaptive

replanning

Testing of the full cooperative system was performed on the

Charles River in Boston, MA, with the autonomous kayaks

shown in Figure 25 that tow acoustic transducers (SSS sen-

sors are simulated). Here, we show results from one partic-

ular set of tests from November 2015. The vehicles travel

on the surface and have access to RTK GPS, but these

data are never used by the vehicles to update estimates: the

vehicles dead reckon and use acoustic measurements only

throughout the mission.

A snapshot of the vehicles on the water is shown in Fig-

ure 26, where an acoustic packet has just been transmitted

and received. The up-to-date means of the probabilistic cov-

erage maps are also shown, as well as the track locations

that are being followed to guarantee coverage. The pose

graph for “Nostromo,” the left-most vehicle in Figure 26,

is shown in Figure 27 and Extension 1.

To evaluate the system we compare four cases.

• Zero error assumption: A structured and pre-defined

path that would be guaranteed to achieve complete

coverage in the absence of any localization drift.

• Worst-case error assumption: A structured and pre-

defined path that is guaranteed to achieve complete cov-

erage under that assumption that the vehicle error is

bounded, similar to Bretl and Hutchinson (2013).

Fig. 26. Two vehicles, Nostromo (yellow) and Kestrel (red), per-

forming a cooperative coverage on the Charles River in Boston,

MA. The blue boxes indicate the areas to be covered, where blue

is less covered and red area is more covered. The cyan circle

indicates an acoustic transmission and the corresponding green

circle indicates successful packet reception. The tracks are also

shown for each vehicle, which are being adaptively updated to

guarantee coverage completion. The dark blue paths behind the

vehicles indicate the poses that are currently in the sliding window

estimate.

Fig. 27. The multi-vehicle pose graph on-board “Nostromo” at

the snapshot in Figure 26. Green triangles indicate own-vehicle

poses, orange triangles are other-vehicle poses at communica-

tions/measurements times, the red lines are the combined odom-

etry factors that have been sent through acoustic packets, and the

pink lines are the relative range factors.

• Adaptive not cooperative: Path planning is adaptive but

vehicles do not communicate.

Paull et al. 39

Fig. 28. Ground-truth paths taken to cover the workspace for the

four different cases by “Nostromo” (yellow vehicle in Figure 26).

Fig. 29. Ground-truth paths taken to cover the workspace for the

four different cases by “Kestrel” (red vehicle in Figure 26).

• Adaptive + cooperative: The full proposed adaptive and

cooperative approach.

Figures 28 and 29 show the paths resulting from these

four cases for the areas shown in Figure 26. The worst-

case error assumption is overly conservative at the start

and results in more transects being required. Comparing

the cooperative and non-cooperative case, the initial track

spacings are similar, but as time passes in the mission, the

track spacings for the non-cooperative case become smaller

due to the fact the position uncertainty is accumulating

more quickly (as shown in the 100% packet loss case in

Figure 23).

Fig. 30. Ground-truth confidence maps resulting from paths in

Figure 28. (a) Adaptive and cooperative. (b) Adaptive not cooper-

ative. (c) Worst-case error assumption. (d) Zero error assumption.

Fig. 31. Ground-truth confidence maps resulting from paths in

Figure 29. (a) Adaptive and cooperative. (b) Adaptive not cooper-

ative. (c) Worst-case error assumption. (d) Zero error assumption.

The resulting coverage (confidence) maps for the paths

shown in Figures 28 and 29 are shown in Figures 30 and

31, respectively. The area is completely covered in all cases

except the zero error assumption.

Finally, we compare the path lengths and final ground

truth coverage percentages in Table 1. We can see that the

full adaptive and cooperative strategy is able to fully cover

the area with the shortest path.

8. Related work

This work combines concepts from the disciplines of

robotics area coverage planning and cooperative localiza-

tion. Some of the more related works are summarized

here.

40 The International Journal of Robotics Research 37(1)

Table 1. Path length and final coverage

Vehicle 1 Vehicle 2

Path length (m) Coverage (%) Path length (m) Coverage (%)

Adaptive + cooperative 1920 99.2 1958 99.0

Adaptive not cooperative 2279 99.2 2278 99.2

Zero error assumption 1363 93.9 1387 93.2

Worst-case error 2565 99.7 2563 99.8

8.1. Area coverage

Area coverage is a sub-problem of the more general path

planning problem. Instead of generating a path that effi-

ciently navigates from a start point to a goal point, the

objective is now to generate a path such that every point

in the workspace is covered with a sensor. For a survey

of existing coverage planning algorithms please refer to

Choset (2001) and the more recent survey by Galceran and

Carreras (2013). These surveys also define important cat-

egorizations of the literature. For example, algorithms are

categorized as either online or offline, and heuristic or com-

plete. Our approach here questions the typical concept of

completeness by arguing that completeness implies per-

fect localization, which was recognized by Choset in earlier

work: “the term complete is used in the motion planning

sense, not in the operating research field sense” (Choset,

2000).

A common approach to the area coverage problem is to

sub-divide the workspace into a set of cells to be covered

and then define some heuristic or structured approach to

cover each individual cell (Choset, 2000). This is appeal-

ing since the connectivity can be expressed as a graph and

graph covering algorithms can be applied. Pioneering work

in the field was done by Choset and Acar (Acar et al.,

2006; Acar and Choset, 2002b; Acar et al., 2002, 2003;

Choset, 2000). This includes the Boustrophedon decom-

position (Choset, 2000), the development to sensor-based

methods based on the online sensing of critical points

(Morse decomposition) (Acar et al., 2002) and then the

extension to robots with extended range sensors (A ⊂ S

in Figure 2). An important point is that these online meth-

ods are constructing the cell decomposition online but not

adapting the structured paths within the cells based on

localization from sensor data.

More recently the classical approaches have been

extended and applied to vacuum cleaning (Baek et al.,

2011) image gathering with an unmanned aerial vehicles

(Paull et al., 2014d), 3D terrain coverage for farming (Jin

and Tang, 2011), agricultural applications (Oksanen and

Visala, 2009), and underwater imaging (Paull et al., 2013),

using a variety of different cell decomposition, and heuristic

techniques (Galceran and Carreras, 2013).

8.1.1. Multiple vehicles. As we argue here, a group of

robots can achieve goals one single robot cannot achieve.

In addition, multi-robot systems can be more efficient and

robust. However, new issues arise when a group of robots

work together, for example: the environment becomes more

complex and dynamic, control must be scalable, coordi-

nation and cooperation algorithms must be developed, and

limited communications channels must be used effectively.

Multi-robot coverage algorithms for ground and aerial

vehicles include: multi-robot Boustrophedon decomposi-

tion (Rekleitis et al., 2000, 2004), distributed partition-

ing (Gasparri et al., 2008), grid-based coverage with task

allocation (Barrientos et al., 2011), multi-robot graph

exploration (Brass et al., 2011), frontier-based exploration

(Nagatani et al., 2011), methods based on the GVD (Breit-

enmoser et al., 2010; Sipahioglu et al., 2010), and coop-

erative coverage with limited range sensing (Fazli et al.,

2010).

8.1.2. Robot uncertainty. In this work, we are remov-

ing the assumption inherent in all “classical” coverage

approaches that the robot is perfectly localized. The main

motivation for this is that without considering the pose

uncertainty, it is possible to become overly confident that

a certain areas have been covered. For example, in the case

of a safety-critical missions this is very important. The gen-

eral navigation with uncertainty problem is well-studied as

it is an instance of a partially observable Markov decision

process (POMDP). In general, these problems are not com-

putationally tractable, but recent works have made progress

in the case of linear Gaussian systems. For example the

belief roadmap (Prentice and Roy, 2009) and feedback

information roadmap (Agha-mohammadi et al., 2014). It is

also possible to navigate “safely” without solving the full

POMDP problem. Lambert and Fort-Piat (2000) achieved

this through a set-bounding approach where the platform’s

3σ ellipse should never come into contact with an obsta-

cle. An alternative approach to defining the safe distance

based on pose uncertainty that is more closely related to

the approach taken here is to define a probability of col-

lision with an obstacle. Paths can be chosen that have the

right combination of optimality and risk (Huang and Gupta,

2009). In the approach of Blackmore et al. (2011), a user

can specify a minimum allowable probability of collision

and then an optimal path is found subject to this so-called

“chance constraint.” In van den Berg et al. (2011), a simi-

lar approach is taken that bridges the gap between planning

and control. A linear-quadratic Gaussian motion planner

is combined with the rapidly exploring random trees plan-

ner to determine a path that minimizes the probability of

Paull et al. 41

collision with obstacles. The path is planned in advance

assuming accurate stochastic models for motion dynamics

and sensor measurements, as well as obstacle locations.

However, it is not clear how these start-to-goal style

POMDP approaches necessarily apply directly to the area

coverage problem. The key difference is that progress

towards the objective (coverage) is a function of the entire

robot trajectory, and not just the current state of the robot.

The first known work to account for navigation uncertainty

in a coverage framework is in Acar and Choset (2002a),

where critical points are recognized to reduce the navi-

gation error induced through DR. Subsequently, in Mazo

and Johansson (2004), it is assumed that position measure-

ments are only available to robots on the boundary of the

workspace, and control strategies are generated to minimize

the number of turns in paths. For some outdoor applica-

tions, it can be assumed that the uncertainty of the robot

is bounded and constant since there is constant access to

GPS measurements. In such a scenario, structured paths

can be moved closer together to account for the known

and constant uncertainty (Lee et al., 2011). The “prob-

ably approximately correct” measure of performance for

stochastic coverage was defined in Das et al. (2011). This

measure defines the probability of coverage of a given frac-

tion of the workspace based on the platform pose uncer-

tainty. However, this approach uses the assumption that

platform localization error is constant. Furthermore, this

“probably approximately correct” measure is actually a spe-

cial case of our general formulation in Section 2.5. In the

approach by Bretl and Hutchinson (2013), the requirement

of a constant uncertainty is removed, however, the uncer-

tainty is still assumed bounded. This planner takes a “worst-

case” approach as opposed to our method of evolving the

planning conservativeness based on the up-to-date estimate

of the coverage and robot state from a MAP estimator.

A floor cleaning application was presented in Hess et al.

(2014). They were able to provide high-confidence guaran-

tees for cleanliness of the floor. However, this work assumes

that the level of coverage (dirtiness) can be sensed directly,

which is not the case for most coverage problems, for exam-

ple the seabed imaging example that we investigate. In

terms of coverage representation, perhaps the most closely

related work is Goel et al. (2013). In this work, a floor clean-

ing application was considered. The workspace was divided

into a grid where each cell in the grid can be either covered,

unknown, or obstacle. In addition, each covered cell has an

associated certainty that was calculated based on the robot’s

position covariance at the time of covering. However, these

values are only used to generate heuristic rules to cover the

floor in a sensible-looking manner.

Particularly related in the marine context is the work of

Galceran et al. (2014), which focused on coverage with an

AUV of underwater structures. An initial plan was gener-

ated based on a prior bathymetric map that incorporated

the robot’s dynamic constraints. Subsequently, paths were

updated based on a trajectory optimization that accounts for

the vehicles uncertainty and the actual underwater structure

observed during mission. In the work of Kim and Eustice

(2015), the area coverage was treated with an active SLAM

context, where coverage and localization uncertainty reduc-

tion were seen as competing objectives. An information-

theoretic framework was proposed to balance these two

objectives and the method was demonstrated through a

ship-hull inspection task.

The only known work to consider multi-robot area cover-

age with uncertainty is Tully et al. (2009), where the authors

showed three robots completing a cooperative area cover-

age task while performing cooperative localization. In the

proposed approach, the vehicles took turns to act as station-

ary beacons for the others. However, this was primarily a

work about cooperative localization and the coverage path

planning was not addressed.

8.2. Cooperative localization

The first known work to exploit relative measurements

between robots for localization was Kurazume et al. (1994)

where members of the team were divided into two groups

that take turns remaining stationary as landmarks for the

other. The term cooperative localization was coined in Rek-

leitis et al. (2000), where the necessity for some robots

to be stationary was also removed. Subsequently, many

authors have suggested different estimation algorithms such

as distributed EKF (Roumeliotis and Bekey, 2002), max-

imum likelihood (Howard et al., 2002), MAP (Nerurkar

et al., 2009), and particle filter (Prorok and Martinoli,

2011). Recently, some works have specifically addressed

the communications bandwidth issue through quantization

of measurement data (Nerurkar and Roumeliotis, 2013;

Nerurkar et al., 2011; Trawny et al., 2009), or estima-

tion of unknown correlations through covariance intersec-

tion (Carrillo-Arce et al., 2013). The quantization-based

approach is based on the sign-of-innovation Kalman filter

and still requires transmission of at least 1 bit for every

real-valued measurement. In addition, these approaches

are not robust to unknown communications failures. The

covariance intersection method in Carrillo-Arce et al.

(2013) can claim linear scalability of data throughput with

the size of the robot team; however, this method was

approximate.

Several methods are capable of handling asynchronous

communications such as those of Leung et al. (2010),

Nerurkar and Roumeliotis (2010), and Carrillo-Arce et al.

(2013). For example, Leung et al. (2010) provided a frame-

work for deciding under what conditions raw data can be

replaced by filtered estimates. Similarly, in Nerurkar and

Roumeliotis (2010) a delayed-state filter was proposed.

These works have two notable shortcomings for implemen-

tation in communication-constrained settings: first, filter-

ing approaches will always require the transmission of the

joint state covariance matrix that scales O( N2) where N

is the size of the robot team; and, second, data backlog

over extended periods of disconnectivity between nodes is

problematic.

42 The International Journal of Robotics Research 37(1)

There are several works that consider cooperative local-

ization in the underwater domain, for example Bahr et al.

(2009a), where one or more support vehicles are referred

to as communications and navigation aids (CNAs), and

Webster et al. (2013) and Walls and Eustice (2013, 2014),

where vehicles are separated into “servers” and “clients.” In

these cases, communications were only one-way and there-

fore consistency is guaranteed by construction. The neces-

sity to transmit a full joint covariance matrix can also be

avoided through the interleaved update approach of Bahr

et al. (2009b), however the estimates from this approach

are overly conservative. In Fallon et al. (2010), a keyframe-

style approach was used, where only a subset of the relative

measurements were used and the remaining communica-

tion slots were used to marshal data. The keyframe rate

is chosen a priori based on the expected performance of

the communication channel. Unexpectedly poor communi-

cation performance or long periods of disconnectivity will

always result in data backlogging and algorithm failure. In

our approach, we transmit raw data, but we combine mea-

surements together to avoid this backlogging problem. The

recent work of Walls et al. (2015) and Toohey et al. (2014),

which were developed in parallel, also solve this problem

similarly.

9. Conclusion

We have presented a cooperative area coverage approach for

the case where the robot state uncertainty is non-negligible

and time-varying. The backbone of the approach is a prob-

abilistic representation of the coverage over the workspace

that explicitly accounts for the robot pose uncertainty at

the time of covering. Given the now-coupled nature of

coverage and pose estimation, we have presented a slid-

ing window estimation approach that closely approximates

the full MAP trajectory estimate. This approach has been

extended to multiple robotic coverers communicating and

cooperatively localizing over an unreliable link. We have

presented an online adaptive coverage planner that operates

over the probabilistic coverage representation to produce

paths that maximize coverage while providing complete-

ness guarantees in the case of bounded path tracking and

consistent localization estimates. Finally, this framework

has been applied to the problem of seabed mapping by an

AUV. The experimental results have shown that we can still

achieve coverage in the case of very high DR error and, fur-

thermore, we have been able to quantify the benefit of the

cooperative approach in terms of a reduction in path lengths

required to achieve complete coverage.

In future work, we will extend the proposed approach to

the full SLAM scenario based on the approach presented in

Paull et al. (2015).

Funding

The author(s) disclosed receipt of the following financial sup-

port for the research, authorship, and/or publication of this article:

This work was partially supported by the National Science and

Engineering Research Council of Canada, the Office of Naval

Research (ONR) (grant number N00014-13-1-0588), NSF (Award

II-S1318392), and ONR Global.

Notes

1. Subsequently we will use the term “coverage sensor,” but it

should be implied that this could be an end effector.

2. The problem of how to perform this decomposition, the area

partitioning problem, is not considered in this work. The inter-

ested reader is referred to Hert and Lumelsky (1998) and Bast

and Hert (2000).

3. Notational conventions: bold face indicates vectors, capital

letters indicate random variables (RVs), and p( x) is used to

represent the probability density function of a RV X .

4. We use the convention w = 0, not covered; w = 1, covered.

5. The inverse function 8−1N ( p) yields the Mahalanobis distance

r such that the probability that a point falls within a distance

of r of the Gaussian is p.

6. This is an extension of the original algorithm proposed in

Paull et al. (2014b) to the more general case of pose in SE( n)

based on the subsequent work of Walls et al. (2015).

7. All code is available for download at https://github.com/

liampaull/PAC

References

Acar E and Choset H (2002a) Exploiting critical points to reduce

positioning error for sensor-based navigation. In: Proceedings

ICRA ’02: IEEE International Conference on Robotics and

Automation, 2002, volume 4. pp. 3831–3837 vol.4.

Acar E, Choset H and Lee JY (2006) Sensor-based coverage

with extended range detectors. IEEE Transactions on Robotics

22(1): 189–198.

Acar EU and Choset H (2002b) Sensor-based coverage of

unknown environments: Incremental construction of Morse

decompositions. The International Journal of Robotics

Research 21: 345–367.

Acar EU, Choset H, Rizzi AA, Atkar PN and Hull D (2002) Morse

decompositions for coverage tasks. The International Journal

of Robotics Research 21(4): 331–344.

Acar EU, Choset H, Zhang Y and Schervish MJ (2003) Path

planning for robotic demining: Robust sensor-based coverage

of unstructured environments and probabilistic methods. The

International Journal of Robotics Research 22(7-8): 441–466.

Agha-mohammadi A, Chakravorty S and Amato N (2014)

Firm: Sampling-based feedback motion-planning under motion

uncertainty and imperfect measurements. The International

Journal of Robotics Research 33(2): 268–304.

Baek S, Lee TK, Se-Young OH and Ju K (2011) Integrated on-

line localization, mapping and coverage algorithm of unknown

environments for robotic vacuum cleaners based on minimal

sensing. Advanced Robotics 25(13-14): 1651–1673.

Bahr A, Leonard JJ and Fallon MF (2009a) Cooperative local-

ization for autonomous underwater vehicles. The International

Journal of Robotics Research 28(6): 714–728.

Bahr A, Walter M and Leonard J (2009b) Consistent cooperative

localization. In: IEEE International Conference on Robotics

and Automation, 2009 (ICRA ’09), pp. 3415–3422.

Barrientos A, Colorado J, del Cerro J, Martinez A, Rossi C, Sanz

D and Valente J (2011) Aerial remote sensing in agriculture: A

Paull et al. 43

practical approach to area coverage and path planning for fleets

of mini aerial robots. Jourbal of Field Robotics 28(5): 667–689.

Bast H and Hert S (2000) The area partitioning problem. In:

Proceedings of the 12th Canadian Conference on Computa-

tional Geometry, Fredericton, New Brunswick, Canada, 16–19

August 2000.

Benjamin M, Newman P, Schmidt H and Leonard J (2009)

An overview of MOOS-IvP and a brief users guide to the

IvP Helm autonomy software. http://dspace.mit.edu/bitstream/

handle/1721.1/45569/MIT-CSAIL-TR-2009-028. pdf.

Blackmore L, Ono M and Williams B (2011) Chance-constrained

optimal path planning with obstacles. IEEE Transactions on

Robotics 27(6): 1080–1094.

Bosse M, Nourani-Vatani N and Roberts J (2007) Coverage

algorithms for an under-actuated car-like vehicle in an uncer-

tain environment. In: 2007 IEEE International Conference on

Robotics and Automation, pp. 698–703.

Brass P, Cabrera-Mora F, Gasparri A and Xiao J (2011) Multirobot

tree and graph exploration. IEEE Transactions on Robotics

27(4): 707–717.

Breitenmoser A, Schwager M, Metzger JC, Siegwart R and

Rus D (2010) Voronoi coverage of non-convex environments

with a group of networked robots. In: 2010 IEEE Interna-

tional Conference on Robotics and Automation (ICRA). pp.

4982–4989.

Bretl T and Hutchinson S (2013) Robust coverage by a mobile

robot of a planar workspace. In: 2013 IEEE International Con-

ference on Robotics and Automation (ICRA), pp. 4582–4587.

Carrillo-Arce L, Nerurkar E, Gordillo J and Roumeliotis S

(2013) Decentralized multi-robot cooperative localization

using covariance intersection. In: 2013 IEEE/RSJ International

Conference on Intelligent Robots and Systems (IROS), pp.

1412–1417.

Choset H (2000) Coverage of known spaces: the Boustrophedon

cellular decomposition. Autonomous Robots 9: 247–253.

Choset H (2001) Coverage for robotics - a survery of recent

results. Annals of Mathematics and Artificial Intelligence 31:

113–126.

Das C, Becker A and Bretl T (2011) Probably approximately cor-

rect coverage for robots with uncertainty. In: 2011 IEEE/RSJ

International Conference on Intelligent Robots and Systems

(IROS), pp. 1160–1166.

Davies G and Signell E (2006) ESPRESSO scientific user guide.

NURC-SP-2006-003, NATO Underwater Research Centre.

Dellaert F and Kaess M (2006) Square root SAM: Simultaneous

location and mapping via square root information smoothing.

The International Journal of Robotics Research 25(12): 1181–

1203.

Englot B and Hover F (2010) Inspection planning for sensor cov-

erage of 3D marine structures. In: 2010 IEEE/RSJ Interna-

tional Conference on Intelligent Robots and Systems (IROS),

pp. 4412–4417.

Fallon M, Papadopoulos G and Leonard J (2010) A measure-

ment distribution framework for cooperative navigation using

multiple AUVs. In: 2010 IEEE International Conference on

Robotics and Automation (ICRA), pp. 4256–4263.

Fazli P, Davoodi A, Pasquier P and Mackworth A (2010) Complete

and robust cooperative robot area coverage with limited range.

In: 2010 IEEE/RSJ International Conference on Intelligent

Robots and Systems (IROS), pp. 5577–5582.

Galceran E, Campos R, Palomeras N, Ribas D, Carreras M

and Ridao P (2014) Coverage path planning with real-time

replanning and surface reconstruction for inspection of three-

dimensional underwater structures using autonomous underwa-

ter vehicles. Journal of Field Robotics 32(7): 952–983.

Galceran E and Carreras M (2013) A survey on coverage

path planning for robotics. Robotics and Autonomous Systems

61(12): 1258–1276.

Gasparri A, Krishnamachari B and Sukhatme G (2008)

A framework for multi-robot node coverage in sensor net-

works. Annals of Mathematics and Artificial Intelligence 52:

281–305.

Goel D, Case J, Tamino D, Gutmann JS, Munich M, Dooley M and

Pirjanian P (2013) Systematic floor coverage of unknown envi-

ronments using rectangular regions and localization certainty.

In: 2013 IEEE/RSJ International Conference on Intelligent

Robots and Systems (IROS), pp. 1–8.

Guo Y and Balakrishnan M (2006) Complete coverage control

for nonholonomic mobile robots in dynamic environments. In:

Proceedings 2006 IEEE International Conference on Robotics

and Automation, 2006 (ICRA 2006), pp. 1704–1709.

Hert S and Lumelsky V (1998) Polygon area decomposition for

multiple-robot workspace division. International Journal of

Computational Geometry and Applications 8: 437–466.

Hess J, Beinhofer M and Burgard W (2014) A probabilistic

approach to high-confidence cleaning guarantees for low-cost

cleaning robots. In: 2014 IEEE International Conference on

Robotics and Automation (ICRA), pp. 5600–5605.

Howard A, Matark M and Sukhatme G (2002) Localization for

mobile robot teams using maximum likelihood estimation. In:

IEEE/RSJ International Conference on Intelligent Robots and

Systems, 2002, vol. 1, pp. 434–439.

Huang A, Olson E and Moore D (2010) LCM: Lightweight com-

munications and marshalling. In: 2010 IEEE/RSJ International

Conference on Intelligent Robots and Systems (IROS), pp.

4057–4062.

Huang G, Kaess M and Leonard J (2013) Consistent sparsifica-

tion for graph optimization. In: 2013 European Conference on

Mobile Robots (ECMR), pp. 150–157.

Huang Y and Gupta K (2009) Collision-probability constrained

PRM for a manipulator with base pose uncertainty. In:

IEEE/RSJ International Conference on Intelligent Robots and

Systems, 2009 (IROS 2009), pp. 1426–1432.

Iagnemma K and Overholt J (2015) Introduction to special issue:

Darpa robotics challenge (DRC). Journal of Field Robotics

32(2): 187–188.

Jin J and Tang L (2011) Coverage path planning on three-

dimensional terrain for arable farming. Journal Field Robotics

28(3): 424–440.

Mazo Jr M and Johansson KH (2004) Robust area coverage using

hybrid control. In: TELEC.

Kaess M and Dellaert F (2009) Covariance recovery from a

square root information matrix for data association. Journal of

Robotics and Autonomous Systems 57: 1198–1210.

Kaess M, Ranganathan A and Dellaert F (2008) iSAM: Incremen-

tal smoothing and mapping. IEEE Transactions on Robotics

24(6): 1365–1378.

Kim A and Eustice RM (2015) Active visual SLAM for robotic

area coverage: Theory and experiment. The International Jour-

nal of Robotics Research 34(4–5): 457–475.

44 The International Journal of Robotics Research 37(1)

Kinsey JC, Eustice RM and Whitcomb LL (2006) A survey

of underwater vehicle navigation: Recent advances and new

challenges. In: Conference on Manoeuvering and Control of

Marine Craft, pp. 1–12.

Kurazume R, Nagata S and Hirose S (1994) Cooperative posi-

tioning with multiple robots. In: Proceedings 1994 IEEE Inter-

national Conference on Robotics and Automation, 1994, pp.

1250–1257.

Lambert A and Fort-Piat NL (2000) Safe task planning integrat-

ing uncertainties and local maps federations. The International

Journal of Robotics Research 19(6): 597–611.

Lee TK, Baek SH, Choi YH and Oh SY (2011) Smooth coverage

path planning and control of mobile robots based on high-

resolution grid map representation. Robotics and Autonomous

Systems 59(10): 801–812.

Leung K, Barfoot T and Liu H (2010) Decentralized localiza-

tion of sparsely-communicating robot networks: A centralized-

equivalent approach. IEEE Transactions on Robotics 26(1):

62 –77.

Li Y, Chen H, Er MJ and Wang XM (2011) Coverage path plan-

ning for UAVs based on enhanced exact cellular decomposition

method. Mechatronics 21: 876–885.

Mannadiar R and Rekleitis I (2010) Optimal coverage of a known

arbitrary environment. In: 2010 IEEE International Conference

on Robotics and Automation (ICRA), pp. 5525–5530.

Nagatani K, Okada Y, Tokunaga N, Kiribayashi S, Yoshida K,

Ohno K, Takeuchi E, Tadokoro S, Akiyama H, Noda I, Yoshida

T and Koyanagi E (2011) Multirobot exploration for search and

rescue missions: A report on map building in RoboCupRescue

2009. Journal of Field Robotics 28(3): 373–387.

Nerurkar E and Roumeliotis S (2010) Asynchronous multi-

centralized cooperative localization. In: International Confer-

ence on Intelligent Robots and Systems, pp. 4352–4359.

Nerurkar E and Roumeliotis S (2013) A communication-

bandwidth-aware hybrid estimation framework for multi-robot

cooperative localization. In: International Conference on Intel-

ligent Robots and Systems, pp. 1418–1425.

Nerurkar E, Roumeliotis S and Martinelli A (2009) Distributed

maximum a posteriori estimation for multi-robot cooperative

localization. In: IEEE International Conference on Robotics

and Automation, 2009 (ICRA ’09), pp. 1402–1409.

Nerurkar ED, Zhou KX and Roumeliotis S (2011) A hybrid

estimation framework for cooperative localization under com-

munication constraints. In: 2011 IEEE/RSJ International

Conference on Intelligent Robots and Systems (IROS), pp.

502–509.

Oksanen T and Visala A (2009) Coverage path planning algo-

rithms for agricultural field machines. Journal Field Robotics

26(8): 651–668.

Papoulis A and Pillai SU (2002) Probability, Random Variables

and Stochastic Processes (4th edn). New York: McGraw-Hill.

Paull L, Huang G, Seto M and Leonard JJ (2015) Communication-

constrained multi-AUV cooperative SLAM. In: 2015 IEEE

International Conference on Robotics and Automation (ICRA),

pp. 509–516.

Paull L, Saeedi S, Seto M and Li H (2013) Sensor-driven

online coverage planning for autonomous underwater vehicles.

IEEE/ASME Transactions on Mechatronics 18(6): 1827–1838.

Paull L, Saeedi S, Seto M and Li H (2014a) AUV navigation and

localization: A review. IEEE Journal of Oceanic Engineering

39(1): 131–149.

Paull L, Seto M and Leonard JJ (2014b) Decentralized coopera-

tive trajectory estimation for autonomous underwater vehicles.

In: 2014 IEEE/RSJ International Conference on Intelligent

Robots and Systems, pp. 184–191.

Paull L, Seto M and Li H (2014c) Area coverage planning that

accounts for pose uncertainty with an AUV seabed survey-

ing application. In: 2014 IEEE International Conference on

Robotics and Automation (ICRA), pp. 6592–6599.

Paull L, Thibault C, Nagaty A, Seto M and Li H (2014d) Sensor-

driven area coverage for an autonomous fixed-wing unmanned

aerial vehicle. IEEE Transactions on Cybernetics 44(9):

1605–1618.

Prentice S and Roy N (2009) The belief roadmap: Efficient plan-

ning in belief space by factoring the covariance. The Interna-

tional Journal of Robotics Research 28(11–12): 1448–1465.

Prorok A and Martinoli A (2011) A reciprocal sampling algo-

rithm for lightweight distributed multi-robot localization. In:

International Conference on Intelligent Robots and Systems,

pp. 3241–3247.

Rekleitis I, Dudek G and Milios E (2000) Multi-robot collabora-

tion for robust exploration. In: IEEE International Conference

on Robotics and Automation, vol. 4, pp. 3164–3169.

Rekleitis I, Lee-Shue V, New AP and Choset H (2004) Lim-

ited communication, multi-robot team based coverage. In: Pro-

ceedings ICRA ’04: 2004 IEEE International Conference on

Robotics and Automation, vol. 4, pp. 3462–3468.

Roumeliotis S and Bekey G (2002) Distributed multirobot local-

ization. IEEE Transactions on Robotics and Automation 18(5):

781–795.

Schneider T and Schmidt H (2013) Model-based adaptive behav-

ior framework for optimal acoustic communication and sensing

by marine robots. IEEE Journal of Oceanic Engineering 38(3):

522–533.

Sibley G, Matthies L and Sukhatme G (2010) Sliding window

filter with application to planetary landing. Journal of Field

Robotics 27(5): 587–608.

Sipahioglu A, Kirlik G, Parlaktuna O and Yazici A (2010)

Energy constrained multi-robot sensor-based coverage path

planning using capacitated arc routing approach. Robotics and

Autonomous Systems 58(5): 529–538.

Smith RC and Cheeseman P (1986) On the representation and

estimation of spatial uncertainty. The International Journal of

Robotics Research 5(4): 56–68.

Thrun S, Burgard W and Fox D (2005) Probabilistic Robotics.

Cambridge, MA: The MIT Press.

Toohey L, Pizarro O and Williams SB (2014) Multi-vehicle

localisation with additive compressed factor graphs. In: 2014

IEEE/RSJ International Conference on Intelligent Robots and

Systems, pp. 4584–4590.

Trawny N, Roumeliotis S and Giannakis G (2009) Cooperative

multi-robot localization under communication constraints. In:

IEEE International Conference on Robotics and Automation,

2009 (ICRA ’09), pp. 4394–4400.

Tully S, Kantor G and Choset H (2009) Leap-frog path design

for multi-robot cooperative localization. In: Field and Ser-

vice Robotics (FSR) (Springer Tracts in Advanced Robotics,

vol. 62). New York: Springer, pp. 307–317.

van den Berg J, Abbeel P and Goldberg KY (2011) LQG-MP:

Optimized path planning for robots with motion uncertainty

and imperfect state information. The International Journal of

Robotics Research 30(7): 895–913.

Paull et al. 45

Walls JM, Cunningham AG and Eustice RM (2015) Cooperative

localization by factor composition over a faulty low-bandwidth

communication channel. In: Proceedings of the IEEE Interna-

tional Conference on Robotics and Automation, Seattle, WA,

pp. 401–408.

Walls JM and Eustice RM (2013) An exact decentralized coopera-

tive navigation algorithm for acoustically networked underwa-

ter vehicles with robustness to faulty communication: Theory

and experiment. In: Proceedings of the Robotics: Science &

Systems Conference, Berlin, Germany.

Walls JM and Eustice RM (2014) An origin state method for com-

munication constrained cooperative localization with robust-

ness to packet loss. The International Journal of Robotics

Research 33(9): 1191–1208.

Webster S, Walls J, Whitcomb L and Eustice R (2013) Decen-

tralized extended information filter for single-beacon coop-

erative acoustic navigation: Theory and experiments. IEEE

Transactions on Robotics 29(4): 957–974.

Webster SE, Eustice RM, Singh H and Whitcomb LL (2012)

Advances in single-beacon one-way-travel-time acoustic nav-

igation for underwater vehicles. The International Journal of

Robotics Research 31(8): 935–950.

Xu A, Viriyasuthee C and Rekleitis I (2011) Optimal complete ter-

rain coverage using an unmanned aerial vehicle. In: 2011 IEEE

International Conference on Robotics and Automation (ICRA),

pp. 2513–2519.

Yang S and Luo C (2004) A neural network approach to complete

coverage path planning. IEEE Transactions on Systems, Man,

and Cybernetics, Part B: Cybernetics 34(1): 718–724. S

Appendix: Index to Multimedia Extension

Archives of IJRR multimedia extensions published prior

to 2014 can be found at http://www.ijrr.org, after 2014

all videos are available on the IJRR YouTube channel at

http://www.youtube.com/user/ijrrmultimedia

Table of Multimedia Extension

Extension Media type Description

1 Video The attachment shows the

autonomous kayaks shown in

Figure 25. Subsequently, the

construction of the probabilistic

coverage maps for two vehicles

are shown in real-time, and

the decentralized pose graph

construction is shown. In the

experiment, all elements of the

system are running in real-time

onboard the vehicles.