IROS 2015 Workshop on On-line decision-making in multi-robot coordination (DEMUR 15)

Transcription

1 IROS 2015 Workshop on On-line decision-making in muli-robo coordinaion () OPTIMIZATION-BASED COOPERATIVE MULTI-ROBOT TARGET TRACKING WITH REASONING ABOUT OCCLUSIONS KAROL HAUSMAN a,, GREGORY KAHN b, SACHIN PATIL b, JÖRG MÜLLER a, KEN GOLDBERG c, PIETER ABBEEL b, GAURAV S. SUKHATME a a Deparmen of Compuer Science, Universiy of Souhern California, Los Angeles, CA 90089, USA b Deparmen of Elecrical Engineering and Compuer Sciences, Universiy of California Berkeley, Berkeley, CA 94720, USA c Deparmen of Indusrial Engineering and Operaions Research, Universiy of California Berkeley, Berkeley, CA 94720, USA corresponding auhor: hausman@usc.edu ABSTRACT. We inroduce an opimizaion-based conrol approach ha enables a eam of robos o cooperaively rack a arge using onboard sensing. In his seing, he robos are required o esimae heir own posiions as well as concurrenly rack he arge. Our probabilisic mehod generaes conrols ha minimize he expeced fuure uncerainy of he arge. Addiionally, our mehod efficienly reasons abou occlusions beween robos and akes hem ino accoun for he conrol generaion. We evaluae our approach in a number of experimens in which we simulae a eam of quadroor robos flying in hree-dimensional space o rack a moving arge on he ground. Our experimenal resuls indicae ha our mehod achieves 4 imes smaller average maximum racking error and 3 imes smaller average racking error han he nex bes approach in he presened scenario. KEYWORDS: Cooperaive muli-robo conrol, arge racking, sensor-based navigaion, rajecory opimizaion. 1. INTRODUCTION Tracking a moving arge has many poenial applicaions in various fields. For example, consider a search and rescue scenario where auonomous ground robos are deployed o assis disaser vicims bu hey are no able o localize hemselves in an unknown environmen. In his case, one could use flying robos ha due o heir higher aliude can ake advanage of, e.g., GPS signals on he one hand and can help o localize he ground robos by observing hem on he oher hand. Alhough i comes a he cos of higher complexiy in moion planning, using muliple cooperaively conrolled robos in his seing provides undispued advanages, such as increased coverage, robusness o failure and reduced uncerainy of he arge. Consider a scenario depiced in Fig. 1 where a eam of aerial robos equipped wih onboard cameras is asked wih racking and esimaing he posiion of a mobile robo wih respec o a global frame of reference. The ideal cooperaive conrol algorihm for his eam would ake ino accoun all visibiliy consrains and uncerainies in order o esablish a configuraion of quadroors ha enables o propagae posiion informaion from he global sensors hrough he quadroors o he arge. The novel conribuions of his paper are as follows: a) we exend he approach from [1] by aking ino accoun sensing disconinuiies caused, for example, by occlusions in differen muli-robo configuraions in a manner ha is amenable o coninuous opimizaion, and b) we generae conrols in 3 dimensions for all he quadroors, hence here is no need for addiional reasoning abou poenial mulirobo sensing opologies. We evaluaed our approach in a number of simulaions and compared i o our previous mehod in which we inroduced cooperaive muli-robo conrol wih swiching of sensing opologies [2]. FIGURE 1. Three quadroors cooperaively racking a arge following a figure-eigh rajecory (blue line). The green ellipsoids show he 3-dimensional covariances of quadroors posiions. The green ellipse represens he 2-dimensional covariance of he posiion of he arge. Magena lines depic he measuremens beween quadroors and he global camera (blue cube a he op). How should he quadroors move in order o minimize he uncerainy of he arge? 2. RELATED WORK The ask of cooperaive arge racking has been addressed in various ways. Many researchers considered cenralized [3, 4], decenralized [5, 6], and disribued [7, 8] approaches o conrol muliple aerial or ground robos. Neverheless, all of he above approaches do no ake ino accoun he posiion uncerainy of he robos ha are deployed o perform he racking ask. The posiion of he robos is assumed o eiher be known or independenly obained wih high accuracy. In his work, we consider boh, he posiion uncerainy of he racked arge as well 1

2 K. Hausman, G. Kahn, S. Pail e al. as he uncerainy in he robos poses, which effecively eliminaes he need of a highly accurae exernal racking sysem. The problem of arge localizaion is very similar o he ask of arge racking. There have been many auhors ha worked on arge localizaion [9 11] in a muli-robo scenario wih onboard sensing. I is worh noing ha hese approaches are implemened in a disribued fashion which makes hem well-suied for muli-robo scenarios wih limied communicaion. One of he simplificaions inroduced in hese approaches, however, is o limi he robos o planar movemens [2] and disable he possibiliy ha he robos can be perceived by each oher. In our work, we relax hese assumpions and show how o cope wih occlusions beween differen robos. 3. APPROACH We propose a cenralized approach where we joinly esimae he posiions of he quadroors and he arge using an EKF. Firs, we describe our sae esimaion echnique o hen focus on he conrol generaion and reasoning abou occlusions STATE ESTIMATION WITH EKF SYSTEM PARAMETRIZATION The sae a ime consiss of he individual quadroor poses x (i), i [1, n] and he arge pose x (arg). Le u (i) be he conrol inpu applied o he ih robo a ime. The join sae and conrol inpu are defined as: x = [x (1),..., x (n), x (arg) ] (1) u = [u (1),..., u (n) ] (2) Le Σ be he uncerainy covariance of he join sae. The dynamics and measuremen models for he join sae are given by he sochasic, differeniable funcions f and h: x +1 = f(x, u, q ), q N(0, Q ) (3) z = h(x, r ), r N(0, R ) (4) where q is he dynamics noise, r is he measuremen noise, and hey are assumed o be zero-mean Gaussian disribued wih sae-dependen covariances Q and R, respecively. We consider wo ypes of sensors and corresponding measuremens: absolue (global) measuremens, e.g., GPS, and relaive measuremens beween wo quadroors or a quadroor and he arge, e.g., disance or relaive pose measuremens. The sochasic measuremen funcion of he absolue sensors is given by: z (i) = h (i) (x (i), r (i) ) while he relaive sensor model is: z (i,j) = h (i,j) (x (i), x (j), r (i,j) ) All measuremen funcions can be naurally exended for he join sae [12] UNCERTAINTY MODEL Given he curren belief (x, Σ ), conrol inpu u and measuremen z +1, he beliefs evolve using an EKF. In order o model he disconinuiy in he sensing domain, which can be caused eiher by a limied field of view or occlusions, we follow he mehod from [1] and inroduce a binary vecor δ R dim[z]. The kh enry in he vecor δ akes he value 1 if he kh dimension of z is available and a value of 0 if no measuremen is obained. We deail he mehod for compuing δ in Sec The EKF updae equaions are as follows: x +1 = f(x, u, 0) + K (z h(x )) Σ +1 = (I K H )Σ +1 (5a) (5b) K = Σ +1 H [ H Σ +1 H + W R W ] 1 (5c) Σ +1 = A Σ A + V Q V A = f x (x, u, 0), V = f q (x, 0) H = h x (x +1, 0), W = h r (x +1, 0), (5d) (5e) (5f) where = diag[δ ] and z is a measuremen obained a ime sep. I is worh noing ha he Kalman gain updae in Eq. 5c includes he binary marix o accoun for disconinuiies in he sensor domains. Furhermore, we apply he Markov assumpion [13] ha he measuremens are condiionally independen given he join sae. Thus, he individual measuremens can be separaely fused ino he belief using he EKF updae equaions DYNAMICS MODEL We assume ha he orienaion of he quadroors is fixed and one can only conrol heir 3D posiion. The dynamics of an individual quadroor is given by: f (i) (x (i), u (i), q (i) ) = x (i) + u (i) + q (i) where x (i), u (i) R 3 are he 3D posiion and velociy and is he lengh of a ime sep. For modeling he arge moion, one can apply a sandard unconrolled moion model. In his work, we assume a consan velociy moion model OBSERVATION MODEL The cameras are assumed o be a he cener of each quadroor facing down. The absolue sensor provides he 3D posiion of he observed quadroor/arge as a measuremen: h (i) (x (i), r (i) ) = x (i) + r (i) The relaive sensor model provides he posiion of he (observed) jh quadroor/arge relaive o he (observing) ih quadroor: h (i,j) (x (i), x (j), r (i,j) ) = (x (j) 3.2. CONTROL GENERATION USING OPTIMIZATION x (i) ) + r (i,j). A each ime sep we seek a se of conrol inpus u :T =+h ha for a ime horizon h minimizes he uncerainy of he 2

3 Opimizaion-based Cooperaive Muli-Robo Targe Tracking F IGURE 2. Signed disance if he quadroor/arge is inside he view frusum (lef) and ouside he view frusum (righ). arge while penalizing collisions and he conrol effor. The final cos funcion is composed as follows: (arg) c (x, Σ, u ) = α r(σ ) + βccollision (x ) + γ u 22 (arg) ct (xt, ΣT ) = α r(σt ) + βccollision (xt ) n n X X ccollision (x) = max( x(j) x(i) 22 dmax, 0) i=1 j=i+1 where α, β, and γ are user-defined scalar weighing parameers and dmax is he maximum disance for which he collision cos akes effec. To eliminae he sochasiciy of he cos funcions, we follow Pla e al. [14] and assume he maximum likelihood observaion is obained a each ime sep. The final objecive funcion is: T 1 X min E[cT (xt, ΣT ) + c (x, Σ, u )] (6) x:t,u:t 1 s.. x+1 = f (x, u, 0) x Xf easible, u Uf easible x0 = xini, Σ0 = Σini We used sequenial quadraic programming (SQP) o locally opimize he non-convex, consrained opimizaion problem [15]. Given he oupu conrols u:t 1 compued using rajecory opimizaion, we follow he model predicive conrol paradigm [16] by execuing a subse of he opimized conrols and hen replanning R EASONING ABOUT O CCLUSIONS The absolue/relaive posiion of a quadroor/arge may no be observable due o occlusions from oher quadroors and he limied field-of-view of he sensor. As previously menioned, we model his disconinuiy wih he binary variable δ (according o [1]). In order o make he objecive funcion differeniable, we approximae δ wih a sigmoid funcion. I is worh noing ha his is only required in he opimizaion sep, he sae esimaion sep remains as defined previously. Le sd(i,j) (x) be he signed disance of x(j) o he fieldof-view of he jh quadroor (see Fig. 2). The signed disance is negaive if he jh quadroor is visible and posiive oherwise. We inroduce he parameer η which deermines he slope of he sigmoidal approximaion. The sigmoidal approximaion of he measuremen availabiliy δ is given by: F IGURE 3. Signed disance funcion in he presence of occlusions. Firs, we deermine he shadows of all he occlusions such ha he resuling field-of-view (he shaded area) is calculaed. The signed disance is compued as he disance o he field-of-view. δ (i,j) = exp[ η sd(i,j) (x)] (7) For more deails on he sigmoidal approximaion of he availabiliy of he measuremen we refer he reader o [1]. To calculae he signed disance of x(j) o he fieldof-view of x(i), we firs represen he field-of-view as a runcaed view frusum wih a minimum and maximum disance given by he sensor model as depiced in Fig. 3. If x(j) is ouside of he view frusum, sd(i,j) (x) is he disance of x(j) o he view frusum as shown in he righ par of Fig. 2. If x(j) is inside he view frusum and here are no occlusions, he signed disance is compued as shown in he lef par of Fig. 2. In he presence of occlusions, we firs deermine he shadows of all he occlusions in he plane of x(j). In he nex sep, we use an open source 2D polygon clipping library - GPC1 o generae he 2D polygon field-ofview, and hen calculae he signed disance. Fig. 3 shows an example of a signed disance funcion in he presence of an occlusion. 4. E VALUATION AND D ISCUSSION We evaluaed our approach in a number of simulaion experimens. The simulaion environmen consiss of a global down-looking camera aached 4 meers above he origin of he coordinae sysem, a ground robo arge and a varying number of quadroors equipped wih down-looking cameras. Each quadroor is conrolled hrough he velociy commands u(i) = [vx, vy, vz ]. The sae of he arge consiss of is posiion and velociy x(arg) = [x, y, vx, vy ] and he arge moves on he XY-plane in a figure-eigh rajecory (see Fig. 1). The lengh of he simulaion ime sep is equal o 0.1s. All camera sensors in his seup have he same properies as described in Sec , and can deec objecs in a 3-meer high runcaed pyramid. Consequenly, he global camera is no able o see he arge direcly. The camera measuremen sandard deviaion is se o 0.02m for ranslaion and 0.001rad for orienaion. In addiion, he measuremen covariance scales quaricly wih he disance o he measured quadroor. In order o make simulaions 1 hp:// 3

4 K. Hausman, G. Kahn, S. Pail e al. race(arge covariance) [m^2] posiion error of arge [m] ime sep [1/10 s] race(arge covariance) [m^2] posiion error of arge [m] ime sep [1/10 s] FIGURE 4. Comparison beween no aking occlusions ino accoun in he opimizaion sep (op) and accouning for occlusions using he signed disance funcion presened in his work (boom). Saisics and 95% confidence inervals over 10 runs wih 3 quadroors. Taking occlusions ino accoun is beneficial especially a he spos righ below he global camera (i.e. sar, middle and he end of he rajecory). race(arge covariance) [m^2] posiion error of arge [m] ime sep [1/10 s] race(arge covariance) [m^2] posiion error of arge [m] ime sep [1/10 s] FIGURE 5. Targe racking resuls for our previous levelbased approach [2] (op) and he hereby presened mehod wihou explicily reasoning abou sensing opologies (boom). Saisics and 95% confidence inervals over 10 runs wih 3 quadroors. more realisic, we also inroduce moion noise wih sandard deviaion equal o 0.1m/s. An example of a sysem seup wih he field of view of each camera is depiced in Fig. 1. Our sysem aims o esimae he posiion of he arge as accuraely as possible by acively conrolling he quadroors EXPERIMENTS SAMPLING VS. OPTIMIZATION EXPERIMENT We compare our opimizaion based conrol generaion wih random sampling and laice planners. Random sampling mehods randomly sample conrols o generae rajecories while laice planners draw samples from a predefined manifold, which in our case was a sphere wih radius equal o he maximum allowed conrol effor. For boh mehods, he cos of each resuling rajecory is evaluaed and he conrols corresponding o he minimum cos rajecory are execued. Fig. 6 shows he saisical resuls of he comparison beween he sampling mehod, laice sampling mehod and our approach for 3 quadroors. In order o make a fair comparison for he sampling mehods, we chose he number of samples such ha he execuion imes per one opimizaion sep of all he mehods were similar and we averaged he resuls over 10 runs. One can noice a significanly larger racking error and he race of he arge covariance in performance of boh of he sampling mehods compared o he rajecory opimizaion approach OPTIMIZATION WITH OCCLUSIONS EXPERIMENT To evaluae he imporance of considering occlusions, we evaluaed our mehod wih and wihou reasoning abou occlusions for a eam of 3 quadroors (Fig. 4) and measured he arge covariance and arge racking error. Of noe is he higher uncerainy in Fig. 4(op) han Fig. 4(boom) a he beginning, middle, and end of he arge rajecory. When no considering occlusions, he error is higher a hese segmens because hese segmens are direcly underneah he global camera, which leads o a crowded space. In his scenario, when no considering occlusions in he opimizaion, he quadroors block each oher s views, resuling in worse racking error as compared o considering occlusions in he opimizaion TOPOLOGY SWITCHING EXPERIMENT We compare he mehod proposed in his paper o our previous arge racking mehod ha inroduced level-based sensing opologies and an efficien opology swiching algorihm [2]. In his approach, he quadroors are organized on differen levels wih an assumpion ha each level can only sense he adjacen level below i. A each ime sep he algorihm deermines he planar conrols for each of he quadroors as well as deermines wheher o swich o one of he neighboring opologies by moving one of he quadroors by one level up or down. This approach was inroduced in order o avoid he reasoning abou occlusions beween quadroors a differen aliudes. In order o make our approach and he level-based approach comparable for sandard quadroors, we inroduce a lengh 3 ime sep delay for he opology swich in order o realisically simulae a real quadroor adjusing aliude. The performance of boh algorihms is depiced in Fig. 5. The quadroors perform beer when explicily reasoning abou occlusions wih our approach. This phenomenon can be explained by he fac ha our novel approach can creae a greaer variey of sensing opologies compared o our previous approach. 4

5 Opimizaion-based Cooperaive Muli-Robo Targe Tracking race(arge covariance) [m^2] posiion error of arge [m] race(arge covariance) [m^2] posiion error of arge [m] race(arge covariance) [m^2] posiion error of arge [m] ime sep [1/10 s] ime sep [1/10 s] ime sep [1/10 s] FIGURE 6. Comparison beween differen sampling approaches and he opimizaion approach presened in his paper. From lef o righ: random sampling, laice sampling and opimizaion. Evaluaion measures are shown wih 95% confidence inervals over 10 runs wih 3 quadroors. The opimizaion approach yields beer resuls han he oher presened mehods. level-based laice planning random sampling our approach avg. max racking error [m] ± ± ± ± avg. racking error [m] ± ± ± ± 7 TABLE 1. Quaniaive resuls for he experimens wih 3 quadroors. Max arge racking error was averaged over 10 runs, arge racking error was averaged over all ime seps and 10 runs. Our approach yields significanly smaller error han he oher baseline mehods AVERAGE ERROR COMPARISONS Table 1 shows he saisics of he average racking error and maximum average racking error for differen approaches esed in our experimens. I is worh noing ha our approach achieved 4 imes smaller average maximum racking error han he nex bes mehod, he level-based approach. Our mehod also achieves a 3 imes smaller average racking error compared o he level-based approach. 5. CONCLUSIONS AND FUTURE WORK We presened an opimizaion-based probabilisic mulirobo arge racking approach ha efficienly reasons abou occlusions. We evaluaed our approach in a number of simulaion experimens. We have compared our mehod o oher baseline approaches such as random sampling, laice sampling, and our previous work on sensing opologies [2]. Our experimenal resuls indicaed ha our mehod achieves 4 imes smaller average maximum racking error and 3 imes smaller average racking error han he nex bes approach in he presened scenario. In fuure work, we plan o exend our cenralized planning approach o fully decenralized, disribued planning. This is advanageous in muli-robo seings wih limied communicaion. Finally, we would like o furher demonsrae he applicabiliy of our approach in a real robo scenario. REFERENCES [1] S. Pail, Y. Duan, J. Schulman, e al. Gaussian belief space planning wih disconinuiies in sensing domains. In Roboics and Auomaion (ICRA), 2014 IEEE Inernaional Conference on, pp IEEE, [2] K. Hausman, J. Müller, A. Hariharan, e al. Cooperaive conrol for arge racking wih onboard sensing. In Inernaional Symposium on Experimenal Roboics (ISER) [3] B. Charrow, V. Kumar, N. Michael. Approximae represenaions for muli-robo conrol policies ha maximize muual informaion. Auonomous Robos 37(4): , [4] J. Fink, A. Ribeiro, V. Kumar, B. Sadler. Opimal robus mulihop rouing for wireless neworks of mobile micro auonomous sysems. In Miliary Communicaions Conference (MILCOM), pp [5] L.-L. Ong, B. Upcrof, T. Bailey, e al. A decenralised paricle filering algorihm for muli-arge racking across muliple fligh vehicles. In Proc. of he IEEE/RSJ In. Conf. on Inelligen Robos and Sysems (IROS), pp [6] E. Adamey, U. Ozguner. A decenralized approach for muli-uav muliarge racking and surveillance. In SPIE Defense, Securiy, and Sensing, pp [7] Z. Wang, D. Gu. Cooperaive arge racking conrol of muliple robos. IEEE Transacions on Indusrial Elecronics 59(8): , [8] B. Jung, G. Sukhame. Cooperaive muli-robo arge racking. In Disribued Auonomous Roboic Sysems 7, pp [9] E. Sump, V. Kumar, B. Grocholsky, P. Shiroma. Conrol for localizaion of arges using range-only sensors. In Journal of Roboics Research 28(6): , [10] G. Hoffmann, C. Tomlin. Mobile sensor nework conrol using muual informaion mehods and paricle filers. IEEE Transacions on Auomaic Conrol 55(1):32 47, [11] B. Grocholsky. Informaion-heoreic conrol of muliple sensor plaforms. Ph.D. hesis, [12] A. Marinelli, F. Pon, R. Siegwar. Muli-robo localizaion using relaive observaions. In Proc. of he IEEE In. Conf. on Roboics & Auomaion (ICRA), pp [13] S. Thrun, W. Burgard, D. Fox. Probabilisic Roboics. MIT Press, [14] R. Pla Jr, R. Tedrake, L. Kaelbling, T. Lozano-Perez. Belief space planning assuming maximum likelihood observaions. In Proc. of Roboics: Science and Sysems (RSS) [15] S. Pail, G. Kahn, M. Laskey, e al. Scaling up gaussian belief space planning hrough covariance-free rajecory opimizaion and auomaic differeniaion. In Inl. Workshop on he Algorihmic Foundaions of Roboics [16] E. F. Camacho, C. B. Alba. Model predicive conrol. Springer Science & Business Media,