Planning in Information Space for a Quadrotor Helicopter in a GPS-denied Environment

Transcription

1 Planning in Informaion Space for a Quadroor Helicoper in a GPS-denied Environmen Ruijie He, Sam Prenice and Nicholas Roy Absrac This paper describes a moion planning algorihm for a quadroor helicoper flying auonomously wihou GPS. Wihou accurae global posiioning, he vehicle mus use onboard sensors o deec environmenal feaures and infer is posiion in a pre-exising map. The vehicle s abiliy o localize iself varies across he environmen, since differen map feaures give differen degrees of localizaion informaion. Therefore, if he vehicle plans a pah wihou regard o how well i can localize iself along ha pah, i runs he risk of becoming los. We begin by applying he Belief Roadmap (BRM) algorihm [1], an informaion-heoreic exension of he Probabilisic Roadmap algorihm, o incorporae sensing during he moion planning process in a high-dimensional space. We exend he original BRM o use non-linear sae inference via he Unscened Kalman Filer (UKF), and describe a sampling algorihm ha minimizes he number of samples required o find a good pah. Finally, we demonsrae he BRM pahplanning algorihm on he helicoper, navigaing in an indoor environmen wih a laser range-finder. I. INTRODUCTION Unmanned air vehicles (UAVs) rely heavily on accurae knowledge of heir posiion for decision-making and conrol. As a resul, considerable invesmen has been made owards improving he availabiliy of global posiioning infrasrucure, including uilizing saellie-based GPS sysem and developing algorihms o leverage exising RF signals such as WiFi. However, imporan domains, such as indoor environmens and he urban canyon, remain wihou access o exernal posiioning sysems. Auonomous UAVs hus currenly have limied abiliy o fly hrough hese areas. Consequenly, here has been considerable growh in he use of environmenal sensors for posiion esimaion on mobile robo sysems. Vehicle localizaion using sonar ranging [2], video imaging [3] and laser ranging [4] have been used exremely successfully in a number of applicaions and have now essenially become commodiy echnology, especially aboard ground robos. Unforunaely, he UAV communiy has no been able o leverage hese ground vehicle successes for wo reasons. Firs, some of he mos successful demonsraions of long-erm robo auonomy have used planar laser ranging based on he ubiquious SICK laser range finder, which normally provides localizaion informaion for hree dimensions wihou addiional specialized hardware. While his is sufficien for ground vehicle localizaion, localizaion in six dimensions during fligh requires considerably more daa. Second, UAVs are severely Ruijie He, Sam Prenice and Nicholas Roy are members of he Compuer Science and Arificial Inelligence Laboraory, Massachuses Insiue of Technology, 77 Massachuses Ave., Cambridge, MA ruijie@mi.edu, prenice@mi.edu, nickroy@mi.edu Ground plane sensor 01 Hokuyo Laser Range finder Fig. 1. imoe2 processor Our quadroor helicoper. consrained by weigh and, consequenly, power limiaions. A vehicle small enough o fly indoors or hrough populaed urban areas safely can carry very lile in erms of sensor payload, leading o reduced range and field of view. Neverheless, mos UAVs can carry some sensing capabiliy for localizaion; hey simply canno carry sensors ha enable hemselves o localize everywhere. If he vehicle can use is sensor model o incorporae prediced measuremens ino is decision making, hen he vehicle can plan rajecories ha are robus o sensor limiaions. In essence, he planner biases he moion of he vehicle owards pahs where he vehicle has high localizaion capabiliy, rading off he value of fas mission compleion agains he cos of errors due o poor localizaion. In his paper, we describe a planning algorihm for he quadroor helicoper, shown in Figure 1, and buil by Ascen Technology [5]. This vehicle has a laser range-finder capable of esimaing posiion, yaw angle and aliude informaion from environmenal feaures wihin a 4m range in a 240 field of view. The limied range and field of view of he sensor lead o posiion esimaes ha vary in accuracy and confidence over he environmen. Our algorihm is based on he Belief Roadmap (BRM) algorihm [1], which is a generalizaion of he Probabilisic Roadmap (PRM) algorihm [6]. The BRM performs searches in he informaion space of he vehicle very efficienly by using he sympleic form of he Exended Kalman Filer (EKF) o find he minimum expeced cos pah for he vehicle. We make wo conribuions in exending he BRM in his paper. Firs, while he original formulaion of he BRM assumed he Exended Kalman Filer for posiion racking, we show how o generalize he BRM o use he Unscened Kalman Filer (UKF) [7] for posiion racking, providing beer approximaion of he non-lineariies of UAV moion and laser sensing. Second, he BRM is a sampling-

2 based planner, and he original formulaion does no provide guidance in choosing a good sampling funcion. We use he noion of a Sensor Uncerainy Field [8], and show how a model of sensor uncerainy can be used o generae a more efficien represenaion of he informaion space. Unlike in previous formulaions, we do no need o compue a dense represenaion of he sensor uncerainy field across he enire configuraion space; by sampling from he sensor uncerainy field, we generae a sparse and compuaionally efficien represenaion of he localizaion performance hrough he environmen. Finally, we conclude he paper wih a demonsraion of he quadroor helicoper using he BRM algorihm o navigae auonomously indoors. II. TRAJECTORY PLANNING We firs formulae he problem of moion planning for a UAV. We assume ha he vehicle is holonomic and ha we have full conrol auhoriy, allowing us o ignore vehicle dynamics and rea he problem as a kinemaic moion planning problem. C denoes he configuraion space [9], he space of all vehicle poses, C free is he se of all collisionfree poses (based on he map M of obsacle posiions) and C obs is he se of poses resuling in collision wih obsacles, so ha C C free C obs. Given an iniial vehicle sae s 0 and a map of he environmen, he planning problem is o find a sequence of acions o move he vehicle from sae s 0 o a goal sae s g wihou collisions. Our UAV has 6 degrees of freedom (x, y, z, roll, pich, yaw), so C = R 6, which is of moderaely high dimension. One popular algorihm for planning in high dimensional spaces is he Probabilisic Roadmap (PRM) algorihm [6], in which a discree graph is used o approximae he conneciviy of C free. If he graph of C free conains he sar and goal saes and a pah exiss from he sar node o he goal node in he graph, hen his pah also exiss in C free. The PRM builds he graph by sampling a se of saes randomly from C (adding he sar sae s 0 and goal sae s g o he sample se), and hen evaluaing each sae for membership in C free ; he assumpion is ha i is much cheaper o evaluae randomly sampled poses in higher dimensions han i is o build an explici represenaion of C free. Samples ha lie wihin C free consiue he nodes of he PRM graph and edges are placed beween nodes where a sraigh line pah beween nodes also lies enirely wihin C free. Given his graph, a feasible, collision-free pah can be found using a sandard graph search algorihm from he sar node o he goal node. The pah can be execued by using a simple conroller o follow each edge o he goal. However, he PRM, and is varians, are no ye well-suied o he problem of a GPS-denied UAV, in ha execuing a plan requires a conroller ha follows he sraigh-line edges joining graph poins. If he UAV execuing he plan does no have a good esimae of is sae, i may no be able o deermine when i has arrived a a graph node and should sar o follow a new edge. III. VEHICLE POSITION ESTIMATION If he UAV does no have access o perfec sae knowledge, such as hrough a GPS sysem, i can sill localize iself by using sensors o measure environmenal feaures and hen regisering hose measuremens agains a pre-exising map. Bayesian saisical analysis is one of he mos robus mehods of localizaion, [2], [10], in which a probabiliy disribuion p(s u 1:,z 1: ) over he (unknown) vehicle sae s a ime is inferred following a series of noisy acions u 1: and measuremens z 1:. Wih some sandard assumpions abou he acions and observaions, he poserior belief can be expressed as p(s u 1:,z 1: ) = 1 Z p(z s ) p(s u,s 1 )p(s 1 )ds 1, (1) S where Z is a normalizaion facor. Equaion (1), referred o as he Bayes filer equaion, provides an efficien recursion for updaing he sae disribuion. We will refer o p(s ) = b as he vehicle s belief abou is posiion. There are many differen represenaions of he Bayes filer equaion, bu he Kalman filer is one of he mos common. The Kalman filer assumes ha all probabiliy disribuions are Gaussian, and ha he ransiion and observaion Gaussians are linearly parameerized by he sae and conrol. The Exended Kalman filer (EKF) allows he same inference algorihm o operae wih non-linear ransiion and observaion funcions by linearizing hese funcions around he curren mean esimae. More formally, he sae s and observaion z are given by he following funcions, s = g(s 1,u,w ), w N(0,W ), (2) and z = h(s,q ), q N(0,Q ), (3) where u is a conrol acion, and w and q are random, unobservable noise variables. The EKF compues he sae disribuion a ime in wo seps: a process sep based only on he conrol inpu u leading o an esimae p(s ) = N(µ,Σ ), and a measuremen sep o complee he esimae of p(s ). The process sep follows as µ = g(µ 1,u ), Σ = G Σ 1 G T + V W V T, (4) where G is he Jacobian of g wih respec o s and V is he Jacobian of g wih respec o w. For convenience, we denoe R V W V T. Similarly, he measuremen sep follows as: µ = µ + K (H µ z ), Σ = (I K H )Σ, (5) where H is he Jacobian of h wih respec o s and K is known as he Kalman gain, given by K = Σ H T ( H Σ H T ) 1 + Q. (6) An alernae form of he EKF represens he covariance by is inverse, he informaion marix [11]. The informaion marix updaes can be wrien as Ω = Σ 1 = (G Σ 1 G T + R ) 1 (7) Ω = Ω + H T Q 1 H. (8) For convenience, we denoe M H T Q 1 H such ha Ω = Ω + M. The disribuion p(s u 1:,z 1: ) can be represened by he informaion vecor ξ and he informaion marix Ω = Σ 1 and may be more efficien o compue in domains where he informaion marix is sparse.

3 IV. BELIEF SPACE PLANNING Recall from secion II ha he PRM planning algorihm consrucs a graph in he sae space C free of he vehicle. However, he vehicle does no know is acual sae bu only has access o he EKF sae esimae b = (µ,σ); by planning in he belief space (or informaion space), he vehicle can disinguish beween sae esimaes where he norm of he covariance is small (i.e., he vehicle has high confidence in is mean sae esimae) and sae esimaes where he norm of he covariance is large (i.e., he mean sae esimae is uncerain). Ideally, beliefs wih high uncerainy are o be avoided, and if encounered, conservaive sensing acion would be a reasonable response. Convenional moion planners generally search for a collision-free pah ha minimizes he disance o he goal locaion. In belief space, every belief ypically has some probabiliy ha he robo is a he goal sae. A more appropriae objecive funcion is herefore o maximize he probabiliy of he goal sae, and exensions of he PRM o opimizaion (as opposed o saisficing, searching for a feasible plan) have been explored elsewhere [12], [13]. A naive approach o planning in belief space would herefore involve sampling beliefs direcly from (µ, Σ), adding he iniial belief b 0 o consruc he graph nodes, placing edges beween pairs of beliefs (b i,b j ) for which a conroller exiss ha can ake he vehicle from belief b i o b j, and hen carrying ou graph search as before. Unforunaely, i has been shown [1] ha he likelihood is zero of sampling any beliefs ha are acually reachable from he iniial belief b 0. However, he EKF represenaion of he belief space carries an exremely useful propery. Each belief b is a facored combinaion of µ and Σ. Under some mild assumpions of unbiased moion and sensor models, he reachabiliy of any µ is a funcion of he vehicle kinemaics and he environmenal srucure as in he PRM. For some µ ha is reachable along a pah from µ 0, he corresponding reachable covariance can be prediced by propagaing he iniial covariance Σ 0 along he pah using equaions (4) and (5) and he moion and sensing models. Therefore, o consruc a graph of he reachable belief space, he planner firs samples a se of mean poses {µ i } from C free using he sandard pose sampling of he PRM algorihm, and places an edge e ij beween pairs (µ i,µ j ) if he sraigh line beween poses is collision-free. Forward search is used o search for a pah hrough he graph, bu each sep of he search compues he poserior covariance a each node insead of he sandard cos-o-go. A. Belief Updaing as a One-Sep Operaion The mos compuaionally demanding aspec of he graphsearch algorihm described above is in propagaing he iniial covariance Σ 0 o each graph node. Covariance propagaion requires muliple EKF updaes along each edge e ij, and while his operaion is a consan muliplier of he asympoic search complexiy, i can sill dominae he overall search ime. Furhermore, hese EKF updaes are no a one-ime cos; he search process will find muliple pahs o node i. Each of hese pahs will lead o a differen poserior covariance a node i, and each such covariance mus be propagaed Sampled nodes and edges Sep 1 Sep 2 Series of process and measuremen updaes Sep 3 Single ransfer funcion ζ ij Node i Edge e ij Node j Fig. 2. The Belief Roadmap wih one-sep ransfer funcions calculaed using he UKF. In sep 1, he graph of mean poses is consruced, and muually visible nodes are conneced wih edges. In sep 2, he poserior covariance is calculaed hrough a series of process and measuremen updaes. In sep 3, he one-sep covariance ransfer funcion is calculaed from he individual muli-sep updaes. along edge e ij o reach node j, incurring he compuaional cos of propagaing along he edge (some series of EKF updaes) for each covariance. The BRM algorihm avoids his complexiy by using an alernae represenaion of he covariance ha allows muliple EKF updaes o be compiled ino single linear ransfer funcion. By pre-compuing he ransfer funcion for each edge, he search complexiy for belief space planning becomes comparable o configuraion space planning. I has been shown previously [14], [1] ha he covariance of a Kalman filer-based sae esimaor can be facored as Σ = BC 1, where he combined process and measuremen updae for an EKF gives B and C as linear funcions of B 1 and C 1. Given: Σ 1 = B 1 C 1 1 (9) Σ = G B 1 C R (10) = (G B 1 )(G T C 1 ) 1 + R (11) ( = D E 1 ) 1 (12) where D = G T C 1 and E = G B 1 + R (G T C 1 ) and equaion (12) follows from a marix inversion lemma. The covariance updae in he informaion form can similarly be facored as Σ = (Σ 1 + H T Q 1 H T ) 1 (13) = (D E 1 + M ) 1 (14) Using he same marix inversion lemma, = E (D + M E ) 1 (15) Σ = B C 1, (16) where B = E = G B 1 + R (G T C 1 ) and C = (D + M E ) = G T C 1 + M G B 1 + R (G T C 1 ). In boh cases, B and C are linear funcions of B 1 and C 1. Collecing erms, we can wrie he complee updae sep linearly, such ha [ [ ] [ ] [ B 0 I 0 G T B Ψ = = C] I M G RG T, (17) C] 1

4 Algorihm 1 The Belief Roadmap (BRM) algorihm. Require: Sar belief (µ 0,Σ 0 ), goal µ goal and map C 1: Sample poses {µ i } from C free o build belief graph node se {n i } such ha n i = {µ = µ i,σ = } 2: Creae edge se {e ij } beween nodes (n i,n j ) if he sraigh-line pah beween (n i [µ],n j [µ]) is collision-free 3: Build one-sep ransfer funcions {ζ ij } e ij {e ij } 4: Augmen node srucure wih bes pah p=, such ha n i ={µ,σ,p} 5: Creae search queue wih iniial posiion and covariance Q n 0 ={µ 0,Σ 0, } 6: while Q is no empy do 7: Pop n Q 8: if n = n goal hen 9: Coninue 10: end if 11: for all n such ha e n,n and no n n[p] do 12: Compue one-sep updae Ψ = ζ n,n Ψ, where Ψ = [ ] n[σ] I 1 13: Σ Ψ 11 Ψ 21 14: if r(σ ) < r(n [Σ]) hen 15: n {n [µ],σ,n[p] {n }} 16: Push n Q 17: end if 18: end for 19: end while 20: reurn n goal [p] where Ψ is he sacked block marix [ B C consising of he ] covariance facors and ζ = [ ] W X Y Z is he one-sep ransfer funcion for he covariance facors for G, H, R and M. Noice ha all of he elemens in ζ are direcly conrollable, excep for M, which is relaed o he measuremen z bu is no a funcion of he measuremen iself. M represens he oal amoun of informaion ha he measuremen provides a ime and depends on he measuremen noise model Q (which is usually consan) and he measuremen Jacobian H. The accuracy of he EKF approximaion assumes ha he measuremen funcion is locally linear, which is exacly he approximaion ha he Jacobian is locally consan. As a resul, whenever he EKF assumpions hold, hen we can assume ha M is consan and known a priori. This allows us o deermine ζ for any poin along a rajecory; furhermore, he lineariy of he updae allows us o combine muliple ζ marices ino a single, one-sep updae for he covariance along he enire lengh of a rajecory. Therefore, for each edge e ij in he BRM graph, we can pre-compue each ζ along he edge from he relevan Jacobians and hen muliply he se of ζ s ino a single ransfer funcion ζ ij ha will propagae an iniial (facored) covariance along he lengh of he edge in a single marix muliply. Figure 2 shows his process of consrucing he ransfer funcion for each edge. Table 1 describes he complee Belief Roadmap algorihm. Sep 2 of he algorihm conains a pre-processing phase where each edge is labeled wih he ransfer funcion ζ ij ha allows each covariance o be propagaed in a single sep. V. THE UNSCENTED KALMAN FILTER The criical sep of he BRM algorihm is he consrucion of he ransfer funcion, which depends on erms R and M, he projecions of he process and measuremen noise erms ino he sae space. R and M also represen he informaion los due o moion, and he informaion gained due o measuremens. When using he Exended Kalman filer o perform sae esimaion, hese erms are rivial o compue. However, he EKF is no always a feasible form of Bayesian filering, especially when linearizing he conrol or measuremen funcions leads o a poor approximaion. A paricularly relevan applicaion where EKF sae esimaion fares poorly is localizaion in discree or grid-based maps. Grid map represenaions conain a srong independence assumpion beween he grid cells, which causes measuremens of neighboring grid cells o appear uncorrelaed. When compuing he Jacobian of a measuremen wih respec o a grid cell, however, he gradien of he measuremen is srongly correlaed wih he neighboring cells. As a resul, EKF localizaion requires high-level feaures such as walls and corners o be exraced for use in boh compuing he innovaion of he measuremens and compuing he Jacobians. This example is one of a number of problems ha can occur wih a sandard EKF implemenaion. In order o address he limiaions of linearizaion, alernae forms of he Bayes filer have been developed. One recen exension is he Unscened Kalman filer (UKF) [7], which uses a se of 2n + 1 deerminisic samples, known as sigma poins from an assumed Gaussian densiy o represen he probabiliy densiy of a space of dimensionaliy n. These samples are generaed according o: X 0 =µ 1, (18) X i =µ 1 + ( (n + λ)σ ) i, i=1,...,n (19) X i =µ 1 ( (n + λ)σ ) i, i=n+1,...,2n (20) ( (n ) i where + λ)σ is he ih column of he roo of he marix. Each sigma poin X i has an associaed weigh wm i used when compuing he mean, and wc i is he weigh used when compuing he covariance, such ha 2n wi c = 1, 2n wi m = 1. The weighs and he λ parameers model he widh of he covariance; he mechanism for choosing hese parameers can be found in [7]. The samples are propagaed according o he non-linear process model such ha X i = g(x i,u,0), (21) generaing he process mean and covariance µ = Σ = 2n 2n w i mx i (22) w i c(x i µ )(X i µ ) + R. (23) The sigma poins are used o creae sigma poins in he measuremen space, which are hen ransformed o generae

5 he poserior mean and covariance (µ,σ ), such ha Z i = h(x i,0) µ z = S = K = ( 2n ( 2n 2n w i m(z i µ z )(Z i µ z ) w i c(x i µ )(Z i µ z ) w i mz i (24) ) ) + Q (25) S 1 (26) µ = µ + K (z µ z ) (27) Σ = Σ K S K. (28) The advanage o he UKF formulaion is ha he process and measuremen funcions are no projeced ino he sae space by a linearizaion; insead, he Unscened Transform compues he momens of he process and measuremen disribuions direcly in he sae space iself. As a resul, he UKF eliminaes he need for linearizaion and capures he disribuion accuraely up o he second order, raher han jus he firs order fideliy of he EKF. Unforunaely, alhough he UKF provides a mechanism of efficienly racking he poserior disribuion as a Gaussian while avoiding linearizaion of he measuremen model, he UKF no longer calculaes he M marix which is a criical piece of he individual ransfer funcions ζ. However, we can sill recover M from he UKF updae direcly by working in he informaion form and noicing ha M is he informaion gain due o measuremen z. We can herefore combine equaion (8) and equaion (28), Ω = Ω + M (29) M = Ω Ω (30) = Σ 1 Σ 1 (31) = (Σ K S K ) 1 Σ 1 (32) In order o calculae he M marix for a series of poins along a rajecory, we can generae a prior covariance and compue he poserior covariance as in equaion (28). Happily, he UKF covariance updae does no depend on he acual measuremen received, exacly like he EKF covariance updae. The UKF is sill a projecion of he measuremen noise ino he sae space, bu is a more accurae projecion han an explici linearizaion of he measuremen model. By represening he belief updae process wih he one-sep ransfer funcion, we are approximaing he non-linear UKF updae. Figure 3(a) depics he difference beween covariances compued using he full UKF updae and covariances compued using he one-sep ransfer funcion for a range of moions and randomized iniial condiions. As expeced, he one-sep ransfer funcion using he M marix calculaed in equaion (32) is an approximaion o he UKF model bu he induced error is low; he race of he covariances are closely mached. The UKF calculaion of he informaion gain M does, however, depend on he specific prior marix Σ. As a resul, differen choices of prior for equaion (32) may resul in differen one-sep ransfer funcions. Figure 3(b) shows a Trace of One Sep UKF Covariance Trace of Full UKF Covariance (a) Comparison of covariance predicions Couns Normalized Error in Bins (b) Disribuion of error using consan prior approximaion Fig. 3. (a) Comparison of race of covariance from full UKF filering and race of covariance from one-sep ransfer funcion using UKF M marix. (b) Disribuion of raio of error induced by compuing he M marix for he one-sep ransfer funcion using a consan prior. disribuion of he raio of he error of he one-sep covariance o he full UKF covariance, where 7000 rials were performed using 100 differen priors and a range of iniial condiions and rajecories were used o calculae he M marix. The error induced in he one-sep ransfer funcion for using a consan M is less han 2% wih a significance of p = 0.955, indicaing low sensiiviy o he choice of prior over a range of operaing condiions. VI. SAMPLING IN BELIEF SPACE As he number of samples and he densiy of he graph grows, he BRM planning process will find increasingly lowcovariances pahs. However, as he densiy of he graph grows, he cos of searching he graph will also grow; searching he graph will have complexiy O(b d ) for b edges per node and pah of lengh d edges. We can reduce his complexiy by minimizing he size of he graph, sampling nodes ha reflec he useful par of he informaion space. The opimal sampling sraegy would generae samples ha lie only on he opimal pah o he goal; his would of course require knowing he opimal pah beforehand. However, some samples are more likely o be useful han ohers: vehicle poses ha generae measuremens wih high informaion value are much more likely o lie on he opimal pah han vehicle poses ha generae measuremens wih lile informaion. If poses are iniially sampled from C uniformly, bu are reained according o he expeced informaion gain from sensing a each poin, he graph will converge o one ha mainains he conneciviy of he

6 (a) Samples drawn uniformly (b) Sample map of sensor uncerainy field (c) Samples from sensor uncerainy field Fig. 4. Bird s-eye view of unsrucured, GPS-denied environmen. The brick srucures are pillars in he underground garage. (a) Disribuion of samples drawn uniformly. (b) The sample map wih he sensor uncerainy field. The inensiy (darkness) of each pixel corresponds o he informaion gain available by sensing here. (c) Disribuion of samples drawn according o he sensor uncerainy field. free space and places nodes ha maximize he localizaion accuracy of he vehicle. We call his sampling sraegy sensor uncerainy sampling, afer he Sensor Uncerainy Field (SUF) defined by Takeda and Laombe [8]. By sampling from his field in building he BRM graph, we gain he benefis of focusing he search on he saes ha lead o high informaion gain wihou he cos of explicily building he sensor uncerainy field. Informaion gain is calculaed from he difference in enropy of he prior and poserior disribuions, Trace of Covariance a Goal Locaion BRM wih SUF Sampling BRM wih Uniform Sampling I(x) = H(p(x z)) H(p(x)) (33) where enropy is H(p(x)) = p(x) log p(x). (34) In pracice, we use a consan prior p(x) = Σ 0 such ha H(p(x)) = C and Bayes rule o compue p(x z) = p(z x) p(x), such ha I(x) = H(p(x z)) C (35) where z = argmax z p(z x) and p(x z) is calculaed according o he UKF. We normalize he poserior enropies so ha I(x) lies in he range [0,1], allowing us o rea he informaion gain of x as a probabiliy ha he sample is acceped or rejeced. Figure 4(a) shows a bird s-eye view of an example environmen wih limied srucure and no GPS, specifically, he Saa Cener parking garage a MIT. The brick srucures in figures 4(a) and (c) are he parking garage pillars and sairwell (op righ). In figure 4(a), sample poses are drawn uniformly. Figure 4(b) shows he sensor uncerainy field [8] where equaion (33) is evaluaed a each locaion (x, y) for fixed heigh and aiude. (The lack of smoohness beween obsacles is an arifac of he rendering process and angular discreizaion.) The pixel inensiy corresponds o he informaion gain, where darker pixels have more informaion. This field is shown only o illusrae he concep; compuing he field for realisic domains is impracical. Finally, figure 4(c) shows samples drawn according o he sensor uncerainy. Noe ha he sample densiy is lowes Number of Samples Fig. 5. Comparison of uniform vs. sensor uncerainy sampling sraegies. The sensor uncerainy sampler finds accurae rajecories wih considerably fewer samples han he uniform sampler. far from he environmenal srucure where sensing provides he leas amoun of informaion. Figure 5 shows he advanage of sampling according o he sensor uncerainy. The graph consruced using sensor uncerainy sampling consisenly found a rajecory resuling in a covariance wih race 1.48 using 100 samples, whereas he uniform sampling mehod required 1000 samples o achieve a covariance of size By sampling uniformly, he sandard BRM requires a large and dense graph o achieve good localizaion accuracy. Table I shows a comparison of graph consrucion and planning imes. The convenional PRM is clearly he fases algorihm in boh graph consrucion speed and pah search, bu he localizaion performance is expecedly poor. The BRM wih sensor uncerainy sampling requires addiional ime during he graph creaion phase, bu his ime can be amorized across muliple queries, and resuls in measurably beer pahs. Trace Goal Covariance Graph Build Time (s) Pah Search Time (s) PRM BRM, Uniform Sampling BRM, Sensor Uncerainy Sampling TABLE I PERFORMANCE AND TIME COSTS OF DIFFERENT PLANNERS.

7 Maximum Range Readings Sensed Obsacle in he X Y plane Field of View of Laser in X Y plane Sensed Ground Plane Field of View of Ground Plane (a) PRM rajecory, uniform sampling (b) BRM rajecory, SU sampling (c) Laser Percepion Model Fig. 6. (a) and (b) show an example indoor environmen for comparing he performance of he BRM algorihm. The dashed line is he rajecory compued by he respecive algorihms, and he solid line is he localizaion esimae of he helicoper s posiion during he execuion of he planned rajecory. The ellipse denoes he uncerainy of he helicoper when i reaches he end goal. (a) The rajecory planned by he PRM and he localizaion esimae of he rajecory flown by he helicoper. (b) The rajecory planned by he BRM using sensor uncerainy sampling and he localizaion esimae of he rajecory flown by he helicoper. (c) The percepion model of he onboard laser range-finder, including he field of view of he X-Y plane and he field of view of he ground plane. VII. INDOOR NAVIGATION RESULTS The BRM algorihm and sensor-maximizing sampling sraegy were esed using he quadroor helicoper, shown in Figure 1. Equipped wih auo-sabilizaion rae gyros and acceleromeers, he helicoper has on-board aiude conrol and hus acs as a sable sensor plaform. The on-board environmenal sensor is a Hokuyo URG laser sensor a planar laser rangefinder ha provides a 240 field-of-view a 10 Hz, up o an effecive range of 3m. The laser is mouned in he X-Y plane of he helicoper, and we modified he laser o opically redirec 20 of is field-of-view o provide a small se of range measuremens in he (downward) z direcion. In a single scan, he vehicle is herefore able o esimae is posiion, yaw orienaion and aliude wih respec o environmenal feaures. Figure 6(c) shows an example scan. In pracice, he measuremen of he ground plane is relaively noisy, alhough sufficien for closed-loop aliude conrol. In his ask, he helicoper is required o plan a pah o raverse from he saring posiion o he end goal, shown in Figures 6(a-b), and mus consanly be able o localize iself while execuing he planned rajecory. To compare he performance of he BRM algorihm using he sensor uncerainy sampling sraegy, as opposed o a radiional PRM algorihm performing uniform sampling, we firs plan a pah for he helicoper using each mehod. We hen manually fly he helicoper hrough he environmen using he planned rajecories, recording boh he helicoper s laser range measuremens and he moion commands ha were ransmied o he helicoper. The sensor and conrol daa were hen used o es he localizaion abiliy of he helicoper when execuing he given planned rajecory. Figure 6(a) shows an example rajecory generaed by he radiional PRM planner, which finds a direc pah from sar o goal. Because his plan ignores he helicoper s need for sensor informaion o localize iself, he helicoper is highly uncerain of is posiion by he end of he execued rajecory, and suffers a localizaion error of 259cm a he goal. On he oher hand, an example BRM rajecory using sensor uncerainy sampling enables he helicoper o say welllocalized, as shown in Figure 6(b). The helicoper achieves his by deouring from he shores pah oward areas of high sensor informaion, resuling in an error a he goal of only 9cm, as well as low uncerainy in is final posiion, as shown by he size of he ellipse in Figure 6(b). This demonsraes ha he BRM rajecory leads o measurably more accurae performance. VIII. RELATED WORK Modern approaches o planning wih incomplee sae informaion are ypically based on he parially observable Markov decision process (POMDP) model [15] or cas as a graph search hrough belief space [16]. Compuing POMDP soluions requires finding an opimal acion for each possible belief in he enire belief space. While he POMDP provides a general framework for belief space planning, as he size of he belief space grows POMDP echniques become compuaionally inracable. The complexiy of he soluion grows exponenially in boh he number of possible conrol oucomes and he number of poenial observaions. Numerous approximaion algorihms coninue o miigae he problem of scalabiliy [17], [18], bu o dae POMDP echniques sill face compuaional issues in addressing large problems. The concep of he informaion gain a each locaion is relaed o he Sensor Uncerainy Field (SUF) [8], which esimaes he disribuion of possible errors in robo configuraion ha may resul from sensing a each locaion in space. A every posiion in he map, he SUF is an esimae of he expeced localizaion error when he robo s sensor daa a sae x is mached agains a prior environmen model. A precompued SUF model herefore provides he pah-planner wih a mehod for selecing safer pahs by racing hose poins ha resul in less posiional uncerainy. Alernaively, he Augmened MDP uses he concep of informaion gain by he sensor a each possible pose in he environmen freespace [19] in order o compue a dense policy. The

8 Augmened MDP approach is srongly relaed o he ideas in his paper, bu does no scale well o more han wo dimensions. The exended Kalman filer and unscened Kalman filer have been used exensively. Ko e al. [20] use he imoe2 echnology and he UKF for sae esimaion in aerial vehicles, and Valeni e al. [21] were he firs o demonsrae reliable navigaion and posiion esimaion on quadroor helicopers. The sympleic form (and relaed Hamilonian form) of he covariance updae has been repored before, originally in [22]. Mos recenly, Mourikis e al. [23] used a semi-definie programming approximaion o opimize he Riccai equaion corresponding o he covariance updae for a se of sensors, solving he problem of opimal measuremen frequencies as a convex opimizaion problem. Finally, laser range finding on-board helicopers is no a novel echnology [24], [25], alhough we believe we are he firs o demonsrae reliable auonomous localizaion and moion planning on an indoor helicoper using laser range finding. IX. CONCLUSION In his paper, we have addressed he problem of a helicoper localizing and navigaing in GPS-denied environmens. The helicoper uses laser range daa and an exising map o localize, bu he laser has a limied field of view, causing he helicoper o lose rack of is own posiion in cerain configuraions and in some pars of he environmen. We showed how he Belief Roadmap algorihm [1] can be used o plan rajecories hrough he environmen ha incorporae a predicive model of sensing, allowing he planner o minimize he posiional error of he helicoper a he goal using efficien graph search. The original BRM algorihm assumed an exended Kalman filer model for posiion esimaion, and we showed how his algorihm can be exended o use he unscened Kalman filer. Furhermore, we showed ha by choosing an appropriae sampling algorihm, he BRM can find beer rajecories wih fewer samples han using uniform sampling sraegies. X. ACKNOWLEDGEMENTS Ruijie He was suppored by he governmen of Singapore, and Sam Prenice and Nicholas Roy were suppored by Draper Laboraories under URPP Robus Disribued Sensor Neworks. This projec was suppored by he Office of he Dean, School of Engineering and he MIT Air Vehicle Research Cener (MAVRC). Jan Sumpf and Daniel Gurdan provided he quadroor helicoper and he suppor of Ascending Technologies. Col. Peer Young, Jonahan How, Spencer Ahrens and Bre Behke provided addiional suppor in he developmen of he vehicle. Dirk Hähnel, Karl Koscher, Jonahan Leser and Adam Rea provided considerable assisance wih he imoe2 package including he MSB and LSB boards. Finally, Inel Labs Seale and Universiy of Washingon donaed he imoe hardware. The auhors wish o hank his large group of people in suppor of his projec. REFERENCES [1] S. Prenice and N. Roy, The belief roadmap: Efficien planning in linear pomdps by facoring he covariance, in Proceedings of he Inernaional Symposium on Roboics Research, [2] J. Leonard and H. Durran-Whye, Mobile robo localizaion by racking geomeric beacons, IEEE Transacions on Roboics and Auomaion, vol. 7, no. 3, pp , June [3] R. Sim and G. Dudek, Comparing image-based localizaion mehods, in Proceedings of he Eigheenh Inernaional Join Conference on Arificial Inelligence (IJCAI), Acapulco, Mexico, [4] S. Thrun, D. Fox, W. Burgard, and F. Dellaer, Robus mone carlo localizaion for mobile robos, Arificial Inelligence, vol. 128, no. 1-2, pp , [5] D. Gurdan, J. Sumpf, M. Achelik, K. Doh, G. Hirzinger, and D. Rus, Energy-efficien auonomous four-roor flying robo conrolled a 1 khz, in Proc. ICRA, [6] L. E. Kavraki, P. Sveska, J.-C. Laombe, and M. Overmars, Probabilisic roadmaps for pah planning in high dimensional configuraion spaces, IEEE Transacions on Roboics and Auomaion, vol. 12, no. 4, pp , [7] S. Julier, J. Uhlmann, and H. Durran-Whye, A new approach for filering nonlinear sysems, in Proceedings of he American Conrol Conference, 1995, pp [8] H. Takeda and J. Laombe, Sensory uncerainy field for mobile robo navigaion, Roboics and Auomaion, Proceedings., 1992 IEEE Inernaional Conference on, pp , [9] T. Lozano-Perez., Spaial planning: A configuraion space approach, IEEE Transacions on Compuers, vol. C-32, no. 2, pp , February [10] D. Fox, W. Burgard, and S. Thrun, Acive Markov localizaion for mobile robos, Roboics and Auonomous Sysems, vol. 25, no. 3-4, pp , [11] S. Thrun, Y. Liu, D. Koller, A. Y. Ng, Z. Ghahramani, and H. Durran- Whye, Simulaneous Localizaion and Mapping wih Sparse Exended Informaion Filers, The Inernaional Journal of Roboics Research, vol. 23, no. 7-8, pp , [12] L. K. Dale, Opimizaion echniques for probabilisic roadmaps, Ph.D. disseraion, Deparmen of Compuer Science, Texas A & M Universiy, December [13] B. Burns and O. Brock, Toward opimal configuraion space sampling, in Proceedings of Roboics: Science and Sysems, Cambridge, MA, [14] D. Vaughan, A nonrecursive algebraic soluion for he discree Riccai equaion, Auomaic Conrol, IEEE Transacions on, vol. 15, no. 5, pp , [15] E. Sondik, The Opimal Conrol of Parially Observable Markov Processes, Ph.D. disseraion, Sanford, [16] B. Bone and H. Geffner, Planning wih incomplee informaion as heurisic search in belief space, Proc. AIPS, pp , [17] J. Pineau, G. Gordon, and S. Thrun, Poin-based value ieraion: An anyime algorihm for POMDPs, Inernaional Join Conference on Arificial Inelligence (IJCAI), vol. 13, [18] T. Smih and R. Simmons, Heurisic search value ieraion for POMDPs, Proceedings of he 20h conference on Uncerainy in arificial inelligence, pp , [19] N. Roy and S. Thrun, Coasal navigaion wih mobile robos, in Advances in Neural Processing Sysems 12, vol. 12, [20] J. Ko, D. Klein, D. Fox, and D. Hähnel, GP-UKF: unscened kalman filers wih gaussian process predicion and observaion models, in Proceedings of he IEEE/RSJ Inernaional Conference on Ineligen Robos and Sysems (IROS), [21] M. Valeni, B. Behke, G. Fiore, J. P. How, and E. Feron, Indoor muli-vehicle fligh esbed for faul deecion, isolaion, and recovery, in Proceedings of he AIAA Guidance, Navigaion and Conrol Conference, [22] J. E. Poer and W. E. van der Velde, Opimum mixing of gyroscope. and sar racker daa, Jour. Spacecraf and Rockes, vol. 5, no. 5, [23] A. Mourikis and S. Roumeliois, Opimal sensor scheduling for resource consrained localizaion of mobile robo formaions, IEEE Transacions on Roboics, vol. 22, no. 5, pp , Ocober [24] S. Thrun, M. Diel, and D. Hähnel, Scan alignmen and 3d surface modeling wih a helicoper plaform, in Proceedings of he Inernaional Conference on Field and Service Roboics, [25] L. O. Mejias, S. Saripalli, P. Cervera, and G. S. Sukhame, Visual servoing of an auonomous helicoper in urban areas using feaure racking, Journal of Field Roboics, vol. 23, no. 3, pp , 2006.