Towards Cooperative Multi-Robot Belief Space Planning in Unknown Environments

Transcription

1 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments Vadim Indelman Abstact We investigate the poblem of coopeative multi-obot planning in unknown envionments, which is impotant in numeous applications in obotics. The eseach community has been actively developing belief space planning appoaches that account fo the diffeent souces of uncetainty within planning, ecently also consideing uncetainty in the envionment obseved by planning time. We futhe advance the state of the at by easoning about futue obsevations of envionments that ae unknown at planning time. The key idea is to incopoate within the belief indiect multi-obot constaints that coespond to these futue obsevations. Such a fomulation facilitates a famewok fo active collaboative state estimation while opeating in unknown envionments. In paticula, it can be used to identify best obot actions o tajectoies among given candidates geneated by existing motion planning appoaches, o to efine nominal tajectoies into locally optimal tajectoies using diect tajectoy optimization techniques. We demonstate ou appoach in a multi-obot autonomous navigation scenaio and show that modeling futue multiobot inteaction within the belief allows to detemine obot tajectoies that yield significantly impoved estimation accuacy. 1 Intoduction Autonomous opeation unde uncetainty is essential in numeous poblem domains, including autonomous navigation, object manipulation, multi-obot localization and tacking, and obotic sugey. As the obot state is neve accuately known due to motion uncetainty and impefect state estimation obtained fom patial and noisy senso measuements, planning futue actions should be pefomed in the belief space - a pobability distibution function (pdf) ove obot states and additional states of inteest. V. Indelman Depatment of Aeospace Engineeing, Technion - Isael Institute of Technology, Haifa 32000, Isael. vadim.indelman@technion.ac.il 1

2 2 Vadim Indelman Belief space planning has been investigated extensively in the last two decades. While the coesponding poblem can be descibed in the famewok of patially obsevable Makov decision pocess (POMDP), which is known to be computationally intactable fo all but the smallest poblems [17], seveal appoaches that tadeoff optimal pefomance with computational complexity have been ecently developed. These appoaches can be segmented into seveal categoies: point-based value iteation methods, simulation based appoaches, sampling based appoaches and diect tajectoy optimization appoaches. Point-based value iteation methods (e.g. [14, 19]) select a numbe of epesentative belief points and calculate a contol policy ove belief space by iteatively applying value updates to these points. Simulation-based appoaches (e.g. [23, 24]) geneate a few potential plans and select the best policy accoding to a given metic. They ae efeed to as simulation-based appoaches, since they simulate the evolution of the belief fo each potential plan to quantify its quality. Sampling based appoaches (e.g. [1, 6, 21]) discetize the state space using andomized exploation stategies to exploe the belief space in seach of an optimal plan. While many of these appoaches, including pobabilistic oadmap (PRM) [13], apidly exploing andom tees (RRT) [15] and RRT* [12], assume pefect knowledge of the state, deteministic contol and a known envionment, effots have been devoted in ecent yeas to alleviate these esticting assumptions. These include, fo example, the belief oadmap (BRM) [21] and the apidly-exploing andom belief tees (RRBT) [1], whee planning is pefomed in the belief space, theeby incopoating the pedicted uncetainties of futue position estimates. We note that simila stategies ae used to addess also infomative planning poblems (see, e.g., [6]). Diect tajectoy optimization methods (including [9, 18, 20, 25]) calculate locally optimal tajectoies and contol policies, stating fom a given nominal path. Appoaches in this categoy pefom planning ove a continuous state and action spaces, which is often consideed moe natual as the obot states (e.g., poses) and contols (e.g., steeing angles) ae not constained to few discete values. Fo example, Platt et al. [20] apply linea quadatic egulation (LQR) to compute locally optimal policies, while Van den Beg et al. [25] develop a elated method using optimization in the belief space and avoiding assuming maximum likelihood obsevations in pedicting the belief evolution. These appoaches educe computational complexity to polynomial at the cost of guaanteeing only locally optimal solutions. While typically, belief space planning appoaches conside the envionment is known, in cetain scenaios of inteest (e.g. navigation in unknown envionments) this is not a feasible assumption. In these cases, the envionment is eithe a pioi unknown, uncetain o changes dynamically, and theefoe should be appopiately modeled as pat of the infeence and decision making pocesses. Such a concept was ecently developed in [8, 9], whee andom vaiables epesenting the obseved envionment have been incopoated into the belief and locally optimal motion plans wee calculated using a diect tajectoy optimization appoach. In [7], the appoach was extended to a multi-obot belief space planning centalized famewok and was used to facilitate active collaboative estimation in unknown envionments. Simulation- and sampling-based appoaches that conside a pioi unknown envionments have also been ecently developed in the context of active SLAM (see, e.g. [4, 24]). A limitation of these appoaches is that the belief only consides the

3 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 3 envionment obseved by planning time and does not eason, in the context of uncetainty eduction, about new envionments to be obseved in the futue as the obot continues exploation. In this wok we alleviate this limitation, consideing the poblem of coopeative multi-obot autonomous navigation in unknown envionments. While it is well known that collaboation between obots can significantly impove estimation accuacy, existing appoaches (e.g. [3, 10, 22]) typically focus on the infeence pat, consideing obot actions to be detemined extenally. On the othe hand, active multi-obot SLAM appoaches (e.g. [2]) typically focus on coodination aspects and on the tade-off between exploing new egions and educing uncetainty by e-obseving peviously mapped aeas (pefoming loop closues). In contast, in this pape we conside the question - how should the obots act to collaboatively impove state estimation while autonomously navigating to individual goals and opeating in unknown envionments? Addessing this question equies incopoating multi-obot collaboation aspects into belief space planning. To that end, we pesent an appoach to evaluate the pobability distibutions of multiple obot states while modeling futue obsevations of mutual aeas that ae unknown at planning time (Figue 1a). The key idea is that although the envionment may be unknown a pioi, o has not been mapped yet, it is still possible to eason in tems of obot actions that will esult in the same unknown envionments to be obseved by multiple obots, possibly at diffeent futue time instances. Such obsevations can be used to fomulate non-linea constaints between appopiate obot futue states. Impotantly, these constaints allow collaboative state estimation without the need fo the obots to actually meet each othe, in contast to the commonly used diect elative pose obsevations that equie endezvous between obots (e.g. [22]). We show how such constaints can be incopoated within a multi-obot belief, given candidate paths that can be geneated by any motion planning method. One can then identify the best path with espect to a use-defined objective function (e.g. eaching a goal with minimum uncetainty), and also efine best altenatives using diect tajectoy optimization techniques (e.g. [9, 18, 25]). 2 Notations and Poblem Fomulation Let xi epesent the pose of obot at time t i and denote by Li the peceived envionment by that obot, e.g. epesented by 3D points, by that time. We let Zi epesent the local obsevations of obot at time t i, i.e. measuements acquied by its onboad sensos, and define the joint state Θ ove obot past and cuent poses and obseved 3D points as Θk. = Xk L k, X k. = {x0,...,x k }. (1) The joint pobability distibution function (pdf) ove this joint state given local obsevations Z0:k. = { Z0 k},...,z and contols u. { 0:k 1 = u 0,...,u k 1} is given by p ( Θ k Z 0:k,u 0:k 1) p(x 0 ) k i=1 [ p ( x i x i 1,u i 1) p(z i Θ o i ) ], (2)

4 4 Vadim Indelman Unknown envionment goal 1 goal 2 p ( k Z0:k,u0:k 1) Tajectoies and mapped Robot aeas by planning time tk Robot 0 (a) (b) Fig. 1: (a) Illustation of the poposed concept. Multi-obot indiect constaints epesenting mutual futue obsevations of unknown envionments ae shown in blue. (b) 3D view of the scenaio fom Figue 4b: Robots opeate in an unknown envionment and follow paths geneated by PRM that have been identified by the poposed appoach to povide the best estimation accuacy upon eaching the goals. One can obseve the mutually-obseved 3D points that induce indiect multi-obot constaints involving diffeent time instances; these constaints have been accounted fo in the planning phase. Robot initial positions ae denoted by maks (at the top of the figue); uncetainty covaiances of obot poses ae epesented by ellipsoids. whee Θi o Θi ae the involved andom vaiables in the measuement likelihood tem p(zi Θ o i ), which can be futhe expanded in tems of individual measuements ( ) z i, j Z i epesenting obsevations of 3D points l j : p(zi Θ o i ) = j p z i, j x i,l j. The motion and obsevation models in Eq. (2) ae assumed to be with additive Gaussian noise, x i+1 = f (x i,u i ) + w i, z i, j = h(x i,l j ) + v i (3) whee w i N (0,Σ w),v i N (0,Σ v), with Σ w and Σ v epesenting the pocess and measuement noise covaiance matices, espectively. We conside now a goup of R collaboating obots, and denote by Θ k the coesponding joint state Θ k. = Xk L k, X k. = {X k } R =1 (4) compising the past and cuent poses X k of all obots, and whee L k epesents the peceived envionment by the entie goup. Assuming a common efeence fame between the obots is established, L k includes all the 3D points in Lk fo each, expessed in that efeence fame. The joint pdf ove Θ k, the belief at planning time t k, can now be witten as b(θ k ). = p(θ k Z 0:k,u 0:k 1 ) R =1 p ( Θ k Z 0:k,u 0:k 1), (5)

5 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 5 whee u 0:k 1 epesents the contols of all obots and is defined as u 0:k 1. = { u 0:k 1 } R =1. The joint belief at a futue time t k+l can now be similaly defined as b(θ k+l ). = p(θ k+l Z 0:k+l,u 0:k+l 1 ), (6) whee u k:k+l 1 ae futue actions fo a planning hoizon of l steps and Z k+1:k+l ae the coesponding obsevations to be obtained. We will discuss in detail how such a belief can be fomulated in the sequel (Sections 3.1 and 3.2). We can now define a geneal multi-obot objective function [ L ] J (u k:k+l 1 ) =. E c l (b(θ k+l ),u k+l ) + c L (b(θ k+l )), (7) l=0 that involves L futue steps fo all obots, and whee c l is the immediate cost function fo the lth step. The expectation opeato accounts fo all the possible futue obsevations Z k+1:k+l. While fo notational convenience the same numbe L of futue steps is assumed fo all obots in Eq. (7), this assumption can be easily elaxed. Ou objective is to find the optimal contols u k:k+l 1 fo all R obots: 3 Appoach u k:k+l 1 = agmin u k:k+l 1 J (u k:k+l 1 ). (8) In this wok we show how to incopoate into belief space planning multi-obot collaboation aspects such that estimation accuacy is significantly impoved while opeating in unknown envionments. Ou appoach extends the state of the at by incopoating into the belief (6) multi-obot constaints induced by multiple obots obseving, possibly at diffeent futue time instances, envionments that ae unknown at planning time. In lack of souces of absolute infomation (such as eliable GPS, beacons, and known 3D points), these constaints ae the key fo collaboatively impoving estimation accuacy. One can then identify best obot actions o motion plans, accoding to Eq. (8), among those geneated by existing motion planning appoaches (e.g. sampling based appoaches), o esot to diect optimization techniques to obtain locally optimal solutions in a timely manne. In this wok, we focus on the fome case, and conside we ae given candidate paths fo diffeent obots (geneated, e.g. by PRM o RRT). A schematic illustation of the poposed appoach is shown in Figue 1a. We stat with a ecusive fomulation of the multi-obot belief (Section 3.1) and then discuss in Section 3.2 ou appoach to incopoate into the multi-obot belief futue constaints that coespond to mutual obsevations of unknown scenes. Evaluating the objective function (7) involves simulating belief evolution along candidate obots paths.

6 6 Vadim Indelman 3.1 Recusive Fomulation of a Multi-Robot Belief We begin with a ecusive fomulation of the multi-obot belief (6), consideing futue contols u 0:k+l 1 fo all obots to be given. These ae detemined fom candidate obot paths that ae being evaluated, o altenatively in the case of diect tajectoy optimization appoaches, the contols ae detemined fom eithe nominal o petubed obot paths (see, e.g. [9] fo futhe details). Given futue contols fo all obots, the multi-obot belief b(θ k+l ) at the lth futue step can be witten ecusively as follows (see also Eq. (2)): b(θ k+l ). = p(θ k+l Z 0:k+l,u 0:k+l 1 ) = ηb(θ k+l 1 ) R =1 p ( xk+l ) ( x k+l 1,u k+l 1 p Z k+l Θk+l) o, (9) whee η is a nomalization constant, and p ( xk+l ( x k+l 1 k+l 1),u and p Z k+l Θk+l o ) ae espectively the motion model and measuement likelihood tems. We now focus on the measuement likelihood tem p ( Zk+l Θ o k+l), noting that it appeas ecusively in Eq. (9), fo each look ahead step. As ealie, this tem epesents senso obsevations of the envionment (epesented e.g. by 3D points), see Eq. (2). Howeve, now, these ae futue obsevations of the envionment to be made accoding to obot s planned motion. It theefoe makes sense to distinguish between the following two cases: (a) obsevation of 3D points fom L k Θ k epesenting envionments aleady mapped by planning time t k, and (b) obsevation of new aeas that wee not peviously exploed by any of the obots. The fome case allows to plan single- and multi-obot loop closues (e.g. as in [9]), i.e. to quantify the expected infomation gain due to e-obsevation of peviously mapped aeas by any of the obots. We focus on the latte case, which has not been investigated, to the best of ou knowledge, in the context of collaboative active state estimation and uncetainty eduction. Since envionments that ae unknown at planning time t k ae consideed, the key question is how to quantify the coesponding measuement likelihood tem. 3.2 Incopoating Futue Multi-Robot Constaints Despite the fact that the envionments (o objects) to be obseved ae unknown at planning time, it is still possible to eason in tems of mutual obsevations of these unknown envionments to be made by diffeent obots, possibly at diffeent futue time instances. We can then fomulate constaints elating appopiate obot states while maginalizing out the coesponding andom vaiables epesenting the unknown envionments. Moe specifically, let us conside obots and mutually obseving at futue times t k+l and t k+ j, espectively, an unknown envionment epesented, e.g., by 3D points L, k+l,k+ j, with 1 j l. The joint pdf involving the coesponding states and these 3D points can be witten as

7 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 7 ( ) ) ( ) p xk+l,x k+ j,l, k+l,k+ j z k+l,z k+ j p (z k+l x k+l,l, k+l,k+ j p z k+ j x k+ j,l, k+l,k+ j We can now maginalize out the unknown 3D points L, k+l,k+ j to get ( ) ( ) p z k+l,z k+ j x k+l,x k+ j p xk+l,x k+ j z k+l,z k+ j = p ( x k+l,x k+ j,l, k+l,k+ j z k+l,z k+ j = (10) )dl, k+l,k+ j, (11) which coesponds to a multi-obot constaint involving diffeent time instances. In the passive poblem setting, i.e. contols and measuements ae given, this constaint is typically a nonlinea function that involves the obot poses, say x i and x j, and the measued constaint z, i, j which is obtained by matching the measuements z i and z j. Typical examples include matching lase scans o images using standad techniques (e.g. ICP, vision-based motion estimation). The coesponding measuement likelihood tem can thus be witten as ( p(z, i, j x i,x j ) exp 1 ( 2 z, i, j g x i,x j ) ) 2 Σv MR whee Σv MR is the coesponding measuement noise covaiance matix, and g is an appopiate measuement function. Fo example, this function could epesent a nonlinea elative pose constaint. Coming back to Eq. (10), while in ou case the futue obsevations ae not given, the easoning is vey simila: we can denote by z, k+l,k+ j the measued constaint that would be obtained by matching z k+l and z k+ j if these wee known, and consideing, as befoe, the match is successful (i.e. not outlie), it is possible to quantify the measuement likelihood (10) as ) ( p (z, k+l,k+ j x k+l,x k+ j exp 1 ( ) ) 2 z, k+l,k+ j g xk+l,x k+ j 2 Σv MR (13) Note the above assumes obots and will obseve the same unknown scene fom futue states xk+l and x k+ j. How to detemine if two futue measuements (e.g. images, lase scans), to be captued fom obot poses xk+l and xk+ j, will be ovelapping, i.e. epesent a mutually obseved a scene? The answe to this question is scenaio specific. Fo example, in an aeial scenaio with obots equipped with downwad looking cameas, it is possible to assess if the images ae ovelapping given obot poses and a ough estimate of height above gound. Gound scenaios allow simila easoning, howeve hee it is moe likely that the same (unknown) scene is obseved fom multiple views (e.g. autonomous diving with a fowad looking camea), and moeove, obstacles, that ae unknown at planning time, may pevent two adjacent views to obseve a mutual scene in pactice. In this pape we assume one is able to pedict if two futue poses will mutually obseve a scene. Specifically, in Section 4 we conside aeial obots with downwad facing cameas and take a simplified appoach, consideing two futue poses (12)

8 8 Vadim Indelman x k+l and x k+ j to ovelap if they ae sufficiently neaby, quantified by a elative distance below a theshold d. Natually, moe advanced appoaches can be consideed (e.g. account also fo viewpoint vaiation) and be encapsulated by an indicato function as in [16] - we leave the investigation of these aspects to futue eseach. Given candidate obot paths it is possible to detemine using the above method which futue views (poses) will ovelap and fomulate the coesponding multi-obot constaints (13). In paticula, multi-obot constaints between obot at time t k+l and othe obots at time t k+ j with 0 j l can be enumeated as ) p (z, k+l,k+ j x k+l,x k+ j. (14) j We can now wite the measuement likelihood tem p ( Zk+l Θ o k+l) fom Eq. (9) as: p ( Zk+l o Θk+l) = l j Θk+l o p j p ( z k+l, j x k+l,l j) p ( z k+l,k+l 1 x k+l,x k+l 1) (z, k+l,k+ j x k+l,x k+ j ). (15) The fist poduct epesents obsevations of peviously mapped 3D points l j L k, with Θk+l o including those 3D points that ae actually visible fom x k+l. The second ( ) tem p z k+l,k+l 1 x k+l,x k+l 1 denotes a constaint stemming fom obot obseving a mutual unknown scene fom adjacent views, while the last poduct epesents multi-obot constaints (14) that coespond to diffeent obots obseving common aeas that have not yet been mapped by planning time t k. See schematic illustation in Figue 1a, whee these futue constaints ae shown in blue. Substituting Eq. (15) into Eq. (9) yields the final expession fo b(θ k+l ): R b(θ k+l ) = ηb(θ k+l 1 ) p ( x ) k+l x k+l 1,u k+l 1 p ( z k+l, j x k+l,l ) j =1 l j Θk+l o p ( z k+l,k+l 1 x k+l k+l 1),x p (z, k+l,k+ j x k+l,x k+ j) ]. (16) j Seveal emaks ae in ode at this point. Fist, obseve that diect multi-obot constaints, whee a obot measues its pose elative to anothe obot, ae natually suppoted in the above fomulation by consideing the same (futue) time index, i.e. p (z, k+l,k+l x k+l,x k+l ). Of couse, being able to fomulate constaints involving also diffeent futue time instances, as in Eq. (16), povides enhanced flexibility since planning endezvous between obots is no longe equied. Second, obseve the fomulation (14) is an appoximation of the undelying joint pdf of multiple views X making obsevations Z of an unknown scene L, since it only consides paiwise potentials. Moe concetely, maginalizing L out, p(x Z) = p(x,l Z)dL, intoduces mutual infomation between all views in X, i.e. any two views in X become coelated. Thus, a moe accuate fomulation than (14) would conside all obot poses

9 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 9 obseving a mutual scene togethe. Finally, one could also incopoate easoning egading (obust) data association, i.e. whethe a match z, k+l,k+ j fom aw measuements (images, lase scans) z k+l and z k+ j is expected to be an inlie, as fo example done in [11] fo the passive case. These aspects ae left to futue eseach. 3.3 Infeence Ove Multi-Robot Belief Given Contols Having descibed in detail the fomulation of a multi-obot belief b(θ k+l 1 ) at each futue time t k+l, this section focuses on simulating belief evolution ove time given obot contols o paths. As discussed in Section 3, this calculation is equied both fo sampling based motion planning and diect tajectoy optimization appoaches. Thus, we ae inteested in evaluating the belief b(θ k+l ) fom Eq. (16) b(θ k+l ) p(θ k+l Z 0:k+l,u 0:k+l 1 ) = N ( Θ k+l,i k+l). (17) which is equied fo evaluating the objective function (7). Obseve that fo conciseness we ae using hee I k+l I k+l k+l and Θ k+l ˆΘ k+l k+l. This pocess involves a maximum a posteioi (MAP) infeence Θk+l = agmax b(θ k+l ) = agmin[ logb(θ k+l )], (18) Θ k+l Θ k+l which also detemines the coesponding infomation matix I k+l = Σ 1 k+l. To pefom this infeence, ecall the ecusive fomulation (9) and denote the MAP infeence of the belief at a pevious time by b(θ k+l 1 ) = N ( Θ k+l 1,I k+l 1). The belief at time t k+l can theefoe be witten as logb(θ k+l ) = Θk+l 1 Θk+l Σ k+l 1 + R =1 [ x k+l f (xk+l 1,u k+l 1 ) 2 log p ( Z o Σ Q k+l Θk+l) ] (19) We now focus on the tem log p ( Zk+l Θ o k+l). Recalling the discussion fom Section 3.2 and Eq. (15), this tem can be witten as log p ( Zk+l o Θk+l) = z k+l, j h(xk+l,l j) 2 + l j Θk+l o Σ v + z k+l,k+l 1 g(x k+l,x k+l 1 ) 2 Σ v + j z, k+l,k+ j g(x k+l,x k+ j ) 2 Σ MR v, (20) whee the motion and measuement models f and h ae defined in Section 2, and the nonlinea function g was intoduced in Eqs. (12) and (13). We note that while hee we conside the measuement noise covaiance Σv MR to be constant, one could go futhe and model also accuacy deteioation, e.g. as the elative distance between obot poses inceases.

10 10 Vadim Indelman We now poceed with the MAP infeence (18), which, if the futue obsevations Zk+l wee known, could be solved using standad iteative non-linea optimization techniques (e.g. Gauss-Newton and Levenbeg-Maquadt): in each iteation the system is lineaized, the delta vecto Θ k+l is ecoveed and used to update the lineaization point, and the pocess is epeated until convegence. Let us fist descibe in moe detail this faily standad appoach, consideing fo a moment the futue measuements Zk+l ae known. The lineaization point Θ k+l is discussed fist. Recalling that we ae to evaluate belief evolution given obot paths, these paths can be consideed as the lineaization point fo obot poses. On the othe hand, in the case of diect tajectoy optimization appoaches, the nominal contols ove the planning hoizon can be used to geneate the coesponding nominal tajectoies accoding to (simila to the single obot case, see, e.g. [9]) x k+l = { f ( x k+l 1,u k+l 1 ), l > 1 f ( ˆx k,u k ), l = 1 (21) The lineaizaiton point fo the landmaks L k Θ k+l (see Section 2) is taken as thei most ecent MAP estimate. We fist lineaize Eq. (19) logb(θ k+l ) = B k+l Θ k+l 2 Σ k+l 1 + R [ F + k+l Θ k+l b 2 k+l log p ( Z Σ Q k+l =1 Θ o k+l) ] (22) and then lineaize the tem log p ( Zk+l Θ o k+l) fom Eq. (20): log p ( Zk+l o Θk+l) = H k+l, j Θ k+l b 2 k+l, j + (23) l j Θk+l o Σ v + G k+l,k+l 1 Θ k+l b k+l,k+l 1 ) 2 Σ v + j G, k+l,k+ j Θ k+l b, k+l,k+ j ) 2 whee the matices F, H and G and the vectos b ae the appopiate Jacobians and ight-hand-side (hs) vectos. The binay matix B k+l in Eq. (22) is conveniently defined such that B k+l Θ k+l = Θ k+l 1. Using the elation Σ 1 Σ T 2 Σ 1 2 to switch fom a 2 Σ to Σ 1 2 a 2 and stacking all the Jacobians and hs vectos into A k+l and b k+l, espectively, we get Σ MR v Θk+l = agmin A k+l Θ k+l b k+l 2. (24) Θ k+l The a posteioi infomation matix I k+l of the joint state vecto Θ k+l can thus be calculated as I k+l = Ak+l T A k+l. This constitutes the fist iteation of the nonlinea optimization. Recalling again that the futue obsevations Zk+l ae unknown, it is not difficult to show [9] that, while the a posteioi infomation matix I k+l is not a function of these obsevations, the equivalent hs vecto b k+l fom Eq. (24) does depend on Zk+l. This pesents dif-,

11 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 11 ficulties in caying out additional iteations as the lineaization point itself becomes a function of the unknown andom vaiables Z k+l. As common in elated woks (e.g. [9, 18, 20, 25]), we assume a single iteation sufficiently captues the impact of a candidate action(s). Altenatively, to bette pedict uncetainty evolution, one could esot to using the unscented tansfomation, as in [5], o to paticle filteing techniques. Futhemoe, fo simplicity in this pape we also make the maximum-likelihood measuement assumption, accoding to which a futue measuement z is assumed equal to the pedicted measuement using the most ecent state estimate. As a esult, it can be shown that the hs vecto b k+l becomes zeo and thus Θ k+l = Θ k+l. We note one could avoid making this assumption altogethe at the cost of moe complicated expessions, see, e.g. [9, 25]. To summaize, the output of the descibed infeence pocedue is a Gaussian that models the multi-obot belief as in Eq. (17): b(θ k+l ) = N ( Θ k+l,i k+l). 3.4 Evaluation of Candidate Paths Given candidate paths fo obots in the goup, one can identify the best candidates by evaluating the objective function J fom Eq. (7) fo diffeent path combinations. Such a pocess involves simulating belief evolution along the candidate paths of diffeent obots in the goup, as discussed in Section 3.3, while accounting fo multiobot collaboation in tems of mutual obsevations of unknown envionments (as discussed in Section 3.2). 4 Simulation Results In this section we demonstate the poposed appoach consideing the poblem of multi-obot autonomous navigation while opeating in unknown GPS-depived envionments. We conside an aeial scenaio, whee each obot has its own goal and the objective is to each these goals in minimum time but also with highest accuacy. This can be quantified by the follwing objective function: J = R =1 [ κ t goal + (1 κ )t ( Σ goal)], (25) whee Σgoal and t goal epesent, espectively, the covaiance upon eaching the goal and time of tavel (o path length) fo obot. The paamete κ [0,1] weights the impotance of each tem. As the envionment is unknown and thee ae no beacons, adio souces o any othe means to eset estimation eo, the obots can only ely on onboad sensing capabilities and collaboation with each othe to educe dift as much as possible. We assume each obot is equipped with camea and ange sensos and can obseve natual landmaks in the envionment, which ae used to estimate obot pose within a standad SLAM famewok. Howeve, since the envionment is unknown ahead of time, these landmaks ae discoveed on the fly while the planning pocess has access only to envionments obseved by planning time (Section 3). Initial elative

12 12 Vadim Indelman poses between the obots ae assumed to be known, such that the obots have a common efeence fame - appoaches that elax this assumption do exist (e.g. [11]). In this basic study we use a state of the at sampling based motion planning appoach, a pobabilistic oadmap (PRM) [13], to discetize the envionment and geneate candidate paths fo diffeent obots ove the geneated oadmap. Figue 2 shows some of these candidate paths consideing a scenaio of two obots stating opeating fom diffeent locations. In each case we also show the belief evolution (in tems of uncetainty covaiance) along each path, calculated as descibed in Section 3.3, and the multi-obot constaints that have been incopoated into the appopiate beliefs (denoted by cyan colo). In the cuent implementation, these constaints, possibly involving diffeent futue time instances, ae fomulated between any two poses with elative distance close than d metes. We use d = 300 metes fo this theshold paamete (in the consideed scenaio the aeial obots height is about 500 metes). Moe advanced methods could be implemented of couse, consideing also viewpoint vaiation and incopoating statistical knowledge. As seen in Figue 2, only in two of the consideed cases (Figues 2b and 2c), obot paths wee sufficiently close to facilitate multi-obot constaints within belief space planning. In pactice, howeve, only in the latte case numeous infomative constaints have been incopoated. Figue 3 compaes between the two tems in the consideed objective function (25), path length and uncetainty upon eaching the goal, fo the candidate paths shown in Figue 2. The lowest pedicted uncetainty covaiances ae obtained fo candidate paths with identified multi-obot constaints as shown in Figue 3b. In paticula, the pedicted uncetainty is educed by about 40% fom 35 metes to below 20 metes fo the fist (ed) obot. Thee is a pice to pay, howeve, in tems of path lengths (o time of aival): as shown in Figue 3a, to attain these levels of uncetainty, the path of the second (gee) obot is not the shotest among the consideed candidate paths. The decision what solution is the best theefoe depends on the paamete κ fom Eq. (25) that weights the impotance of each tem in the objective function. Next, we conside actual pefomance while navigating to pe-defined goals in unknown envionments using as contols the identified obot paths in the planning phase descibed above. The esults ae shown in Figue 4 fo two altenatives fom Figues 2a and 2c. Only the latte included multi-obot constaints within planning. One can obseve that also in pactice, using contols fom Configuation C dives the obots sufficiently close to make mutual obsevations of 3D points (that wee unknown at planning time) and as a esult significantly impove estimation accuacy fo both obots (see Figues 4c and 4d, and Figue 1b fo a 3D view). 5 Discussion and Futue Wok Results fom the pevious section indicate estimation accuacy can be significantly impoved by modeling multi-obot mutual obsevations of unknown aeas within belief space planning. Moe geneally, we believe simila easoning can be used to impove multi-obot collaboation aspects while opeating also in uncetain, possibly dynamic, envionments.

13 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments Goal 2000 Goal Y [m] Goal Y [m] Goal X [m] (a) Configuation A X [m] (b) Configuation B 2000 Goal 2000 Goal Y [m] Goal Y [m] Goal X [m] (c) Configuation C X [m] (d) Configuation D Fig. 2: Diffeent candidate paths fo ed and geen obots calculated ove a PRM. Robot initial positions ae denoted by maks; each obot has to navigate to a diffeent goal, while opeating in an unknown envionment. The figues show the covaiance evolution along each path. Multi-obot constaints have been incopoated (denoted by cyan colo) wheneve obot poses ae sufficiently close, which happens mainly in (c); as a esult, uncetainty covaiances ae dastically educed. Note these constaints involve diffeent futue time instances. Covaiances wee atificially inflated by a constant facto fo visualization - actual values ae shown in Figue 3. In this basic study we have made seveal simplifying assumptions and did not addess some of the challenges that ae expected to aise in pactical applications. Obstacles: While initially the envionment is unknown, it may be that afte some time obstacles ae identified as the obots continue in exploation. These obstacles can be efficiently avoided upon discovey by discading appopiate paths, as commonly done in sampling based appoaches. Scalability: Although cuent implementation uses PRM, ou appoach can be fomulated within any motion planning algoithm. The combinatoial poblem associated with evaluating candidate tajectoies of diffeent obots is a topic of futue eseach. We note appoaches addessing elated poblems have been actively developed in ecent yeas (e.g. [16]). An inteesting diection is to also

14 14 Vadim Indelman Path length [m] Robot 1 Robot 2 Squae oot pos. cov. [m] Robot 1 Robot A B C D Configuation numbe (a) Path length 0 A B C D Configuation numbe (b) Tace of squae oot final position covaiance Fig. 3: Quantitative compaison between the fou altenatives shown in Figue 2: (a) Path length; (b) covaiance upon eaching the goals. Multi-obot constaints lead to lowest pedicted uncetainty epesented by Configuation C fom Figue 2c. conside genealization of BRM and RRBT to the multi-obot case. A complimentay aspect is to conside diect tajectoy optimization appoaches, which could allow educing sampling esolution. Belief consistency: While hee we conside a centalized appoach, decentalized o distibuted appoaches ae often moe suitable in pactice fo numeous easons. Resoting to these achitectues equies the beliefs maintained by diffeent obots to be consistent with each othe. 6 Conclusions We pesented an appoach fo collaboative multi-obot belief space planning while opeating in unknown envionments. Ou appoach advances the state of the at in belief space planning by easoning about obsevations of envionments that ae unknown at planning time. The key idea is to incopoate within the belief constaints that epesent multi-obot obsevations of unknown mutual envionments. These constaints can involve diffeent futue time instances, theeby poviding enhanced flexibility to the goup as endezvous ae no longe necessay. The coesponding fomulation facilitates an active collaboative state estimation famewok. Given candidate obot actions o tajectoies, it allows to detemine best tajectoies accoding to a use-defined objective function, while modeling futue multi-obot inteaction and its impact on the belief evolution. Candidate obot tajectoies can be geneated by existing motion planning algoithms, and most pomising candidates could be futhe efined into locally optimal solutions using diect tajectoy optimization appoaches. The appoach was demonstated in simulation consideing the poblem of coopeative autonomous navigation in unknown envionments, yielding significantly educed estimation eos.

15 Towads Coopeative Multi-Robot Belief Space Planning in Unknown Envionments 15 (a) Configuation A (b) Configuation C Position nom eo [m] , Config. C 1, Config. C 2, Config. A 1, Config. A Squae oot pos. cov. [m] , Config. C 1, Config. C 2, Config. A 1, Config. A Pose index (c) Position estimation eos Pose index (d) Tace of squae oot position covaiance Fig. 4: Autonomous navigation to goals accoding to identified obot paths in the planning phase. The envionment, epesented by a spase set of landmaks, is initially unknown and only gadually discoveed. Figues (a) and (b) show obot tajectoies and landmak obsevations using paths defined, espectively, by Configuation A and C (see Figue 2). The latte involves numeous mutual obsevations of landmaks, that induce indiectly multi-obot constaints. A 3D view is also shown in Figue 1b. Figues (c) and (d) show the coesponding estimation eos and developing covaiance ove time, which, in oveall, agee with the pedicted belief evolution fom Figue 3b. 7 Acknowledgments This wok was patially suppoted by the Technion Autonomous Systems Pogam. Refeences 1. A. By and N. Roy. Rapidly-exploing andom belief tees fo motion planning unde uncetainty. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages , W. Bugad, M. Moos, C. Stachniss, and F. Schneide. Coodinated multi-obot exploation. IEEE Tans. Robotics, 2005.

16 16 Vadim Indelman 3. L. Calone, M. Kaouk Ng, J. Du, B. Bona, and M. Indi. Rao-Blackwellized paticle filtes multi obot SLAM with unknown initial coespondences and limited communication. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages , S. M. Chaves, A. Kim, and R. M. Eustice. Oppotunistic sampling-based planning fo active visual slam. In IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages IEEE, R. He, S. Pentice, and N. Roy. Planning in infomation space fo a quadoto helicopte in a gps-denied envionment. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages , G. A. Hollinge and G. S. Sukhatme. Sampling-based obotic infomation gatheing algoithms. Intl. J. of Robotics Reseach, pages , V. Indelman. Towads multi-obot active collaboative state estimation via belief space planning. In IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Septembe V. Indelman, L. Calone, and F. Dellaet. Towads planning in genealized belief space. In The 16th Intenational Symposium on Robotics Reseach, Singapoe, Decembe V. Indelman, L. Calone, and F. Dellaet. Planning in the continuous domain: a genealized belief space appoach fo autonomous navigation in unknown envionments. Intl. J. of Robotics Reseach, 34(7): , V. Indelman, P. Gufil, E. Rivlin, and H. Rotstein. Distibuted vision-aided coopeative localization and navigation based on thee-view geomety. Robotics and Autonomous Systems, 60(6): , June V. Indelman, E. Nelson, N. Michael, and F. Dellaet. Multi-obot pose gaph localization and data association fom unknown initial elative poses via expectation maximization. In IEEE Intl. Conf. on Robotics and Automation (ICRA), S. Kaaman and E. Fazzoli. Sampling-based algoithms fo optimal motion planning. Intl. J. of Robotics Reseach, 30(7): , L.E. Kavaki, P. Svestka, J.-C. Latombe, and M.H. Ovemas. Pobabilistic oadmaps fo path planning in high-dimensional configuation spaces. IEEE Tans. Robot. Automat., 12(4): , H. Kuniawati, D. Hsu, and W. S. Lee. Sasop: Efficient point-based pomdp planning by appoximating optimally eachable belief spaces. In Robotics: Science and Systems (RSS), volume 2008, S. M. LaValle and J. J. Kuffne. Randomized kinodynamic planning. Intl. J. of Robotics Reseach, 20(5): , D. Levine, B. Ludes, and J. P. How. Infomation-theoetic motion planning fo constained senso netwoks. Jounal of Aeospace Infomation Systems, 10(10): , C. Papadimitiou and J. Tsitsiklis. The complexity of makov decision pocesses. Mathematics of opeations eseach, 12(3): , S. Patil, G. Kahn, M. Laskey, J. Schulman, K. Goldbeg, and P. Abbeel. Scaling up gaussian belief space planning though covaiance-fee tajectoy optimization and automatic diffeentiation. In Intl. Wokshop on the Algoithmic Foundations of Robotics, J. Pineau, G. J. Godon, and S. Thun. Anytime point-based appoximations fo lage pomdps. J. of Atificial Intelligence Reseach, 27: , R. Platt, R. Tedake, L.P. Kaelbling, and T. Lozano-Péez. Belief space planning assuming maximum likelihood obsevations. In Robotics: Science and Systems (RSS), pages , S. Pentice and N. Roy. The belief oadmap: Efficient planning in belief space by factoing the covaiance. Intl. J. of Robotics Reseach, S.I. Roumeliotis and G.A. Bekey. Distibuted multi-obot localization. IEEE Tans. Robot. Automat., August C. Stachniss, G. Gisetti, and W. Bugad. Infomation gain-based exploation using aoblackwellized paticle filtes. In Robotics: Science and Systems (RSS), pages 65 72, R. Valencia, M. Mota, J. Andade-Cetto, and J.M. Pota. Planning eliable paths with pose SLAM. IEEE Tans. Robotics, J. Van Den Beg, S. Patil, and R. Alteovitz. Motion planning unde uncetainty using iteative local optimization in belief space. Intl. J. of Robotics Reseach, 31(11): , 2012.