Sampling-Based Real-Time Motion Planning under State Uncertainty for Autonomous Micro-Aerial Vehicles in GPS-Denied Environments

Transcription

1 Sensors 2014, 14, ; do: /s Arcle OPEN ACCESS sensors ISSN Samplng-Based Real-me Moon Plannng under Sae Uncerany for Auonomous Mcro-Aeral Vehcles n GPS-Dened Envronmens Dachuan L 1, *, Qng L 1, Nong Cheng 1,2 and Jngyan Song 1 1 Deparmen of Auomaon, snghua Unversy, Bejng , Chna; E-Mals: lqng@snghua.edu.cn (Q.L.); ncheng@snghua.edu.cn (N.C.); jysong@snghua.edu.cn (J.S.) 2 Naonal Key Laboraory on Flgh Vehcle Conrol Inegraed echnology, Flgh Auomac Conrol Research Insue, X an , Chna * Auhor o whom correspondence should be addressed; E-Mal: DachuanL86@gmal.com; el.: ; Fax: Exernal Edor: Felpe Jmenez Receved: 31 July 2014; n revsed form: 26 Ocober 2014 / Acceped: 3 November 2014 / Publshed: 18 November 2014 Absrac: hs paper presens a real-me moon plannng approach for auonomous vehcles wh complex dynamcs and sae uncerany. he approach s movaed by he moon plannng problem for auonomous vehcles navgang n GPS-dened dynamc envronmens, whch nvolves non-lnear and/or non-holonomc vehcle dynamcs, ncomplee sae esmaes, and consrans mposed by unceran and cluered envronmens. o address he above moon plannng problem, we propose an exenson of he closed-loop rapd belef rees, he closed-loop random belef rees (CL-RB), whch ncorporaes predcons of he poson esmaon uncerany, usng a facored form of he covarance provded by he Kalman fler-based esmaor. he proposed moon planner operaes by ncremenally consrucng a ree of dynamcally feasble rajecores usng he closed-loop predcon, whle selecng canddae pahs wh low uncerany usng effcen covarance updae and propagaon. he algorhm can operae n real-me, connuously provdng he conroller wh feasble pahs for execuon, enablng he vehcle o accoun for dynamc and unceran envronmens. Smulaon resuls demonsrae ha he proposed approach can generae feasble rajecores ha reduce he sae esmaon uncerany, whle handlng complex vehcle dynamcs and envronmen consrans.

2 Sensors 2014, Keywords: moon plannng; mcro-aeral vehcles; rapdly explorng random rees (RR); sae esmaon uncerany 1. Inroducon Auonomous mcro-aeral vehcles (MAV) are playng an ncreasngly mporan role n many cvl and mlary applcaons. MAVs ha are capable of auonomously makng decsons and operang can be appled n many asks scenaros ha are no accessble by humans and ground moble robos, such as ndoor exploraon and mappng, search and rescue, dsaser relef, ec. As a resul, here has been an ncreasng neres n developng auonomous MAV sysems for ndoor navgaon. In recen years, many researchers have developed and mplemened varous knds of MAV sysems demonsrang varous degrees of auonomy n performng asks, such as ndoor exploraon [1], envronmen mappng [2] and agle flgh [3]. Despe he consderable progress acheved n hs doman, here are sll many challenges n developng fully auonomous MAV sysems. One of he key problems s he moon/pah plannng for MAVs capable of operang n GPS-dened complex envronmens. For MAVs performng asks n such scenaros, he moon plannng algorhm mus comply wh consrans, ncludng complex vehcle dynamcs (non-lnear and/or non-holonomc dynamcs wh a hgh dmensonal sae space) and envronmenal consrans (unsrucured and unceran, me-varyng operang envronmens). Parcularly, snce MAVs ypcally canno drecly ge access o he nformaon of he curren sae, hey mus esmae he dsrbuon over he saes usng measuremens from onboard sensors or exernal ads (GPS [4], exernal moon capure sysem [5]). However, GPS s unrelable n urban canyons and compleely unavalable n mos ndoor envronmens; he exernal moon capure sysem s also mpraccal for such scenaros, snce requres pre-nsallaon of camera arrays n he envronmen. As a resul, he sae esmaon of MAVs mus rely only on onboard sensng capables, whch s hghly consraned n erms of precson and range due o he sze and wegh consrans of he MAVs. Moreover, he performance of MAV sae esmaon and localzaon usng exerocepve sensors (laser rangefnder [6], camera [7] and RGB-D sensor [8]) vares across he envronmen dependng on he dsncve feaures and percepual srucure of he envronmens. As a resul, he moon plannng progress mus also negrae he uncerany of sae esmaon o ensure relably and robusness o mperfec and nosy sae esmaes caused by lmed sensng capably. hese challenges requre ha he moon/pah planner mus be able o generae feasble pahs ha sasfy he consrans mposed by unceran and unsrucured envronmens, as well as complex vehcle dynamcs, whle ensurng measuremen gaherng along he pah o reduce sae esmaon uncerany. In hs paper, we propose a real-me moon plannng sraegy (closed-loop random belef rees, CL-RB) for auonomous vehcles wh complex dynamcs and n he presence of sae esmaon uncerany, whle handlng he consrans mposed by he unceran and dynamc operang envronmens. he proposed moon plannng sraegy s bul upon he randomzed samplng-based moon framework, he real-me closed-loop rapdly explorng random rees (CL-RR) [9], whch operaes by samplng n he npu space of he conroller and generaes rajecores hrough forward closed-loop smulaon usng

3 Sensors 2014, he vehcle dynamcs model and pah-rackng conroller. hs allows he moon planner o easly accoun for non-lnear and non-holonomc vehcle dynamcs and dynamc unceran envronmens. We exend he general CL-RR o handle he sae uncerany by ncorporang he predcon of poseror sae esmaon uncerany n he moon plannng framework. he uncerany of sae esmaon s characerzed usng a facored form of he covarance [10] provded by he Kalman fler class-based esmaor. hs facored form enables effcen covarance propagaon n ha combnes he updae of covarance along a pah from mul-sep observaons no a sngle lnear sep, and he poseror covarance of a ceran pah afer addng new nodes can also be updaed onlne. he overall moon plannng operaes by ncremenally consrucng a ree of dynamcally feasble rajecores n real me, whle connuously selecng and execung rajecores ha are a radeoff beween mnmzng pah cos and reducng localzaon uncerany. We valdaed he proposed algorhm n varous llusrave scenaros of a smulaed quadroor MAV wh non-lnear dynamcs and lmed sensng n enclosed and unsrucured GPS-dened envronmens. Smulaon resuls demonsrae ha he CL-RB can effcenly generae dynamcally feasble rajecores ha ensure sae esmaon accuracy, whle preservng he propery of he CL-RR framework. he CL-RB can handle complex vehcle dynamcs and sae uncerany, enablng he MAV o auonomously navgae n unceran, unsrucured and GPS-dened envronmens. he paper s organzed as follows: Secon 2 revews relaed work on moon plannng under uncerany. Secon 3 presens he formulaon of he moon plannng problem. Secon 4 provdes he covarance propagaon approach. A dealed descrpon of he CL-RB s presened n Secon 5, followed by he smulaon resuls and analyss n Secon 6. Fnally, he paper s concluded n Secon 7, wh a dscusson on fuure work. 2. Relaed Work Convenonally, moon plannng approaches under sae uncerany are ypcally formulaed as a parally observable Markov decson process (POMDP) problem [11], whch provdes he mos general mahemacal framework for solvng he plannng problem wh paral observably. Whle POMDP has been appled o low-dmensonal and small-scale problems [12], mos POMDP-based approaches rely on dscrezng he sae space, makng compuaonally nracable for realsc applcaons. Recenly, many approaches have been proposed o address he problem of scalably usng approxmaon and erave mehods. Van den Berg e al. [13] proposed an erave moon plannng approach o solve he connuous POMDP problem, usng a belef space varan of he erave LQG (lnear-quadrac Gaussan) mehod. Smlarly, Ba e al. [14] also formulaed he problem as a connuous POMDP and solved he problem wh a Mone Carlo value eraon mehod. However, hese approaches are sll less effecve a addressng problems wh complex vehcle dynamcs and moon plannng wh large-scale, hgh dmensonal sae space or confguraon space. In conras, samplng-based moon plannng sraeges have been wdely acknowledged as effecve approaches for solvng moon plannng problems wh hgh dmensonal confguraon space and complex vehcle dynamcs. In parcular, he PRM (probablsc roadmap) [15] and RR (rapdly explorng random rees) [16] and her varans are he mos wdely appled samplng-based approaches, and hey have been successfully appled n a number of applcaons. More recenly, he closed-loop RR [9,17,18] exends

4 Sensors 2014, he convenonal RR by ncorporang forward predcon usng closed-loop dynamcs model wh conroller. he CL-RR can generae more feasble pahs ha accoun for complex vehcle dynamcs, and can be mplemened n real-me o handle dynamc, unceran envronmens. Several approaches have been proposed o ncorporae sae uncerany n he samplng-based moon plannng framework. Van den Berg e al. [19] proposed he LQG-MP sraegy by negrang he a pror dsrbuon over sae esmaes no he sandard RR framework. However, he LQG-MP algorhm s nracable for real-me applcaons, due o s hgh compuaon cos. In [20], Bry e al., combned an LQG-based covarance prunng echnque wh he RR* framework for plannng n belef space. Alernave o RR-based approaches, he belef roadmap (BRM) exends he PRM by negrang he predcons of sae esmaon uncerany usng an EKF (exended Kalman fler) esmaor and performs belef plannng by searchng pahs wh he lowes uncerany from he roadmap. he BRM was laer exended o use he UKF esmaor and heursc samplng sraegy [21], and was furher mplemened on a quadroor and proven successful for ndoor flgh ess [22]. However, he BRM assumes ha he vehcle s fully conrollable and reas he problem as a smple knemac moon plannng problem, e.g., does no consder complex vehcle dynamcs and he feasbly of he generaed pahs. As a resul, s no ye well sued o vehcles wh complex dynamc consrans and dynamc envronmens. In addon, he BRM operaes by plannng on pre-consruced sac graphs, and herefore, s no a real-me plannng algorhm n essence. Recen research has also consdered anoher smlar knd of plannng problem wh uncerany, where he planner mus generae pahs ha maxmze nformaon collecon and reduce he uncerany on he locaon esmaes of feaures or arges n he envronmen. he nformaon-rch RR (IRR) [23] addresses hs pah plannng problem by ncorporang he qualfcaon of nformaon gan no he CL-RR framework, usng he Fsher nformaon marx. hs knd of problem can be regarded as he nverse problem of plannng wh uncerany on he vehcle s own sae. 3. Problem Formulaon 3.1. Moon Plannng under Sae Uncerany Consder a vehcle wh non-lnear dynamcs ha operaes n an envronmen whou exernal posonng sysems. he vehcle ypcally does no have drec access o he accurae nformaon of s curren sae, bu nsead obans he esmaes of he sae usng observaons derved from he measuremens of onboard sensors, whch are ypcally nosy and ncomplee. he sochasc dynamcs and observaon model of he sysem can be gven by he followng dscree me sae-ranson form: x = + 1 f( x, u, w) w ~ N(0, Q) (1) z = f( x, v ) v ~ N(0, R) where x X, u U and z Z are he vehcle s sae, conrol npu and observaon, respecvely. w and v denoe he process dsurbance and measuremen nose, and boh can be formulaed as Gaussan nose wh zero mean and covarance (Q and R, respecvely). Assumng ha all probably dsrbuons are Gaussan, gven he prevous observaon (z1:) and conrol npus (u1:), he sae s ypcally esmaed usng a Bayesan fler, provdng he dsrbuon of he vehcle s sae (or he belef of he sae): p( x ) = p( x u, z ) 1: 1:

5 Sensors 2014, whch s characerzed by a mean sae μ and a covarance Σ(b(x) = (μ, Σ)). he mean μ provded by he Bayesan fler s a opmal esmaon of he vehcle s sae dsrbuon, and can be used for conrol and decson makng, whle he mean Σ characerzes he confdence (or uncerany) of sae esmaon. Denong Xfree as he subse of all collson-free saes, gven he nal sae x0 Xfree and he goal regon Xgoal X free, as well as he paral knowledge of he envronmen, hen he prmary objecve of moon plannng s o fnd he conrol polcy u0: = π[b(x0:)] and correspondng sequence of saes x0:, such ha he vehcle reaches he goal regon n a fne me horzon by applyng he conrol polcy: x X, (0, ], (0, ) goal f f meanwhle mnmzng he followng objecve funcon: Jb (( x )) E [ C( x x )] C( x, u ) = + b( xgoal ) b( x ), u0: goal = 0 where C(x xgoal) s he expeced cos funcon from x o xgoal; noe ha akes he expecaon form, snce he measuremen and sae are boh probablsc, and C (x, u) s he cos by applyng conrol u. he generaed rajecory mus also sasfy he consrans mposed by he vehcle dynamcs Equaon (1) and envronmens (.e., collson avodance, x 0: X free ). Moreover, when he vehcle does no have accurae knowledge of he esmae of s sae, he moon planner mus ake he uncerany of he sae esmae along he generaed pahs no accoun, hus he moon plannng problem no he belef plannng problem (.e., plannng n belef space [20]). By negrang he nformaon from he mean and covarance of he belef, he moon plannng algorhm mus choose acons and belefs, such ha he poseror covarance a he end of he rajecory s mnmzed, yeldng rajecores ha acheve a rade-off beween low pah cos and hgh confdence n he sae esmaon Sae Esmaon of Gaussan Sysems Consder a sysem wh he dynamc and observaon model n he form of Equaon (1), one of he mos common and robus mehods for esmang he dsrbuon over s sae s he Bayesan flerng [10]. Gven knowledge of he pror conrol npu u and sensor measuremen z+1, he belef of he sae b(x+1) afer a sequence of conrol and observaon can be esmaed as: b( ) = p(, ) = p( ) p(, ) b( ) d x+ 1 x+ 1 u1: z1: λ z+ 1 x+ 1 x+ 1 x u x x (2) where λ s a normalzaon facor. Assumng he sae and observaon ranson funcon (f, h) are lnear wh sae and observaon, whch are boh Gaussan dsrbuons; he Bayesan flerng can be mplemened as he Kalman fler [24], whle he exended Kalman fler (EKF) [25] s ypcally appled for esmang he sae dsrbuon of he sysem wh nonlnear sae and observaon ranson funcons. For a sysem model of he form of Equaon (1), he EKF sae esmaon can be dvded no he process sep and measuremen updae sep. Denong he dsrbuon of he sae b( x ) ( ˆ = N x, Σ), he process sep frs predcs he sae usng he conrol and sae of he prevous sep, whch s gven as: xˆ ( ˆ = g x 1, u ) (3) Σ = G Σ G + V Q V 1

6 Sensors 2014, where G s he Jacoban of g wh respec o he sae, and V s he Jacoban of g wh respec o w. hen, he measuremen sep adjuss he esmae and covarance by ncorporang he nformaon from new observaons: xˆ = xˆ + K ( z H xˆ ) Σ =Σ KHΣ (4) where H denoes he Jacoban of h wh respec o he sae, and K s he Kalman gan, whch s updaed by: K =Σ H ( R + H Σ H ) ypcally, he conrol and decson makng are based on he mean x ˆ of he esmaed dsrbuon provded by he EKF, and he covarance, whle he covarance Σ capures he uncerany of he sae esmae usng he sensor measuremens. herefore, he uncerany of he sae esmae along he rajecory from he pah planner can be evaluaed usng he norm of he covarance, whch wll be dscussed n he nex secon. 4. Lnear Covarance Propagaon In order o evaluae he sae uncerany resulng from a specfc planned rajecory, he mos common approach for a sysem wh he EKF esmaor s o compue he poseror covarance of he endng belef b(x) from he sequence of acons and measuremens along he rajecory. However, he propagaon of he poseror covarance requres mulple erave calculaons of he EKF process and measuremen updang from he nal belef accordng o Equaon (4), leadng o a heavy compuaonal cos. hs s even worse when he nal belef s modfed, snce he poseror covarance mus be re-compued usng he enre EKF updaes from he new nal belef. In parcular, hs fac has a more sgnfcan effec on a samplng-based moon planner, snce generally consrucs a graph and ree of mulple rajecores, hus dfferen pahs can resul n dfferen poseror covarance o he same node, hs requres ha he moon planner mus perform he propagaon process for each covarance. o reduce he compuaonal cos of he covarance propagaon, we rely on prevous resuls on he facorzaon of he covarance [10], whch allows he propagaon of poseror covarance o be propagaed n a sngle lnear updae sep nsead of mulple non-lnear updaes for he EKF fler. Followng heorem 1 n [10], he facorzaon of covarance s gven by: Σ =Λ Π 1 where Λ and П can be calculaed as lnear funcons of Λ 1 and П 1, usng he EKF process and measuremen updae sep. he facorzaon of he covarance marx can be proved usng he followng marx nverson lemma: Lemma 1. For marces 1, 2, 3 R n M M M n, we have: ( M1+ M2M3 ) = M3( M2 + M1M3) (5) Denoe he nal sae covarance as Σ0, and Σ0 can be facored as: 1 Σ 0 =Σ0I (6)

7 Sensors 2014, Gven Σ 1 = Λ 1П 1 1 and denong S = VQV, he process updae of EKF (Equaon (3)) can be wren as: 1 1 G G S G G S G ( G Σ = Σ + = Λ Π + = Λ Π ) + S Followng Equaon (5), we have: ( ) ( )( ) 1 ( 1 ) 1 Σ 1 = G Π GΛ + SG Π = EF = FE (7) where E = G Π 1 and F = GΛ 1 SG + Π 1, For compuaon consderaons, he covarance updae of he measuremen updae sep can be gven by he nformaon form [11]: Ω =Σ = ( GΩ G + ) Ω =Ω H R H Denong N = H R 1 H, Equaon (8) can be rewren as Ω =Ω + N. herefore, from Equaons (7) and (8), we can wre: Followng Equaon (5), where: Σ =Ω = ( Σ + N ) = ( EF + N ) Σ = F( E + N F) =ΛΠ 1 1 (8) Λ = F = GΛ + SG Π 1 1 Π = E + N F = G Π + N ( GΛ + SG Π ) = NGΛ + ( I+ NS) G Π 1 1 (9) As can be seen from Equaon (9), Λ, П are boh lnear funcons of Λ 1, П 1. Denong Ψ as he sacked block marx of Λ and П: Λ Ψ= Π (10) Equaon (10) can be rewren as: Λ 0 I 0 G Λ Ψ = = = Ψ 1 I N ς Π G SG Π 1 1 (11) where: 0 I 0 G G SG ς = I N = G SG NG G + NSG (12) s he one-sep ransfer funcon marx. herefore, he covarance a can be recovered usng he facors of Ψ: 1 Σ =ΛΠ (13) Usng he above facorzaon and he lnear ransfer funcon, he non-lnear Kalman updae process of he covarance can be ransformed no a lnear propagaon sep of he covarance facors, and he mulple updaes can be combned no a sngle ransfer sep. hs allows for effcen covarance propagaon and uncerany predcon for he samplng-based moon plannng sraegy: he poseror

8 Sensors 2014, covarance Σ resulng from any nal belef (μ, Σ) along he rajecory can be recovered by mulplyng mulple ransfer funcons: ς... ς ς Ψ = + 1Ψ = Ψ (14) and n a samplng-based pah planner ha represens pahs n he form of rees and nodes, for any arbrary node nj n he rajecory, he poseror covarance of he sae a he node can be propagaed from he predecessor node j 1 along he pah n one effcen sep usng he ransfer funcon ς0:j (Fgure 1): Ψ j = ς j... ς1ψ 0 = ς0: jψ 0 (15) Fgure 1. Lnear covarance predcon procedure usng he ransfer funcon. ( μ0, Σ0, Ψ0) ς 1 Ψ = ς... ς Ψ = ς Ψ j j 1 0 0: j 0 ς ( μ, Σ, Ψ) ς, j ( μ j, Σ j, Ψ j) akng advanage of he above lnear covarance propagaon approach, he sae esmaon covarance of one pon n (node or me sep) can be predced usng he covarance of s adjacen pon, as well as he EKF Jacoban marx se assocaed wh he rajecory beween n 1 and n; hs procedure can be gven as follows: Sep 1: Exrac he covarance facors Λ 1 and П 1 from block marx Ψ 1 of n 1. Sep 2: Compue he EKF marx se (S, G and N) based on he sae ranson model and observaon model, as well as he predced sae and smulaed observaon (Equaon (1)). Sep 3: Compue he ransfer funcon ς usng S, G and N, accordng o Equaon (12). Sep 4: Propagae he block marx Ψ of n based on ς and Ψ 1, accordng o Equaon (11), hen exrac he covarance facors Λ and П from block marx Ψ. Sep 5: Recover he covarance Σ of n usng Λ and П: Σ = ΛП 1. hs covarance propagaon procedure allows he pah plannng algorhm o ncorporae he covarance-based qualfcaon of sae esmaon uncerany no he plannng progress n an effcen way. In he CL-RB algorhm, whch s descrbed n deal n he followng secon, he sae esmaon uncerany s qualfed usng he race of he covarance: J( Σ ) = r( Σ ) (16)

9 Sensors 2014, Closed-Loop Random Belef rees Algorhm (CL-RB) 5.1. Daa Srucure Smlar o he convenonal RR, he CL-RB operaes by consrucng a ree of rajecores n he sae space (more precsely, he belef space). he ree s defned by nodes and edges ha connec dfferen nodes. he prmary nformaon sored n a node n consss of he mean of he sae, uncerany qualfcaon, he pah cos and he paren node: where μ and Σ represen he mean and he covarance of he sae dsrbuon, respecvely. Ψ s he facored form of he covarance. J(Σ) qualfes he uncerany along he rajecory form he roo node o node n, whch can be calculaed usng he covarance propagaon process descrbed n Secon 4: J(Σ) = r(σ); C(n) sores he daa of he pah execuon cos assocaed wh node n, whch can be spl no wo pars: C ( ) 1 2ˆ( ) = ωc n nroo + ω c xgoal n where c(n nroo) denoes he accumulaed execuon cos resulng from followng he rajecory form ree roo nroo o n, and cx ˆ( ) goal n s he esmaed cos from n o he goal sae xgoal (cos-o-go). ω1, ω2 [0,1] are wegh facors. n.paren represens he ndex for he paren node of n. Every wo neghbor nodes (n, nj) n he ree s conneced by an edge e(n, nj), whch represens he closed-loop predcon rajecory {x...xj} and conrol polcy o seer he vehcle from x o xj ree Expanson he CL-RB moon plannng sraegy s bul upon he sandard closed-loop RR (CL-RR) orgnally proposed by [9] and laer exended n [17,18]. Smlar o he CL-RR, our CL-RB algorhm can be spl no wo prmary processes: a ree expanson process ha explores he envronmen by growng he ree and a pah selecon and execuon loop ha selecs and execues he opmal poron of he ree. Deals of he ree expanson process are descrbed n Algorhm 1. Unlke he sandard RR ha generaes canddae conrol npus randomly or from a look-up able, he ree expanson process of he CL-RB predcs he feasble rajecory by ncorporang a closed-loop sysem model conssng of he rajecory-followng conroller and he vehcle dynamcs model (Fgure 1). Once he nnear s deermned, he planner generaes a reference npu ˆr (Lne 7) usng nnear and xsamp; hen ˆr s sen o he closed-loop sysem conssng of he conroller and he vehcle dynamcs model: he conroller generaes he conrol npuû (Lne 8), whch s hen sen o he vehcle dynamcs o predc he sae oupu ˆx by forward smulaon (Lne 9). he predced rajecory s hen checked agans envronmenal consrans (e.g., collson avodance) o ensure feasbly. he CL-RB algorhm exends he CL-RR ree expanson by ncorporang he predcon of he sae (poson) uncerany n growng he ree, usng he EKF esmaor and he covarance propagaon approach (Secon 4). Frs, a node xsamp Xfree s generaed by random samplng n he free space (Lne 1). Afer ha, he predcon of sae esmaon uncerany s ncorporaed as heursc nformaon

10 Sensors 2014, n he neares-node selecon: he neares node nnear o xsamp by a ceran merc s deermned from he curren ree usng he followng hybrd heursc nformaon: * = arg mn λ1c( n nroo ) + λ2c( xsamp n ) + λ3 ˆ x. ˆ samp Jsamp n. J n. J (17) where λ1, λ2, λ3 are weghng facors for each em. he above heursc nformaon consss of hree componens: he exploraon heursc, he opmzaon heursc and he uncerany heursc. Algorhm 1. Closed-loop random belef ree: ree expanson. 1 ake a sample x n he reference space 2 Fnd he neares node se N near = {n near() }, ( 1) of x samp from ree, usng he hybrd heursc conssng of he pah cos and uncerany qualfcaon 3 for each node n near n he neares node se N near 4 k 0 5 x ˆ( + k) fnal sae of N near 6 whle xˆ( + k) χ free ( + k) and xˆ( + k) has no reached x samp do 7 Generae reference npu r ˆ( + k) from n near o x samp 8 Generae conrol npu u ˆ( + k) from feedback conrol law 9 Smulae x ˆ( + k+ 1) from sae propagaon model 10 k k end whle 12 Generae he predced rajecory x(), [ 0, 1] 13 Spl he predced rajecory x() and add nermedae nodes n free 14 f x() X, [ 0, 1] hen 15 Add x sample and all nermedae nodes o, break 16 else f all nermedae nodes are feasble 17 Add nermedae nodes o, break 18 end f 19 end for 20 for each newly added feasble node n do 21 Updae pah execuon cos esmaes for n (Algorhm 2) 22 smulae he observaons z () and ransfer funcon along he feasble rajecory beween n and n.paren 23 Updae he marx Ψ n and poseror covarance Σ n and uncerany cos J(Σ) of n 24 Updae he oal cos of n 25 end for he exploraon heursc cx ˆ( ) samp n denoes he esmaed cos by connecng n o he sample xsamp, whch enables he pah plannng algorhm o focus on addng new nodes o he ree and quckly explorng he envronmen.

11 Sensors 2014, he opmzaon heursc c(n nroo) denoes he accumulaed cos of he pah from he roo node nroo o he canddae node n. he purpose of usng hs opmzaon heursc s o bas he ree growh owards pahs ha reduce he overall accumulaed cos. In order o ncorporae he sae esmaon uncerany facor no he pah plannng progress, he CL-RB adds he uncerany heursc n he neares node selecon of he ree expanson. hs uncerany heursc enables he moon planner o selec he node ha leads o he lowes poseror sae esmaon uncerany f s conneced o xsamp, usng he predced sae covarance based on he smulaed closed-loop sae oupu and observaons along he rajecory beween canddae nodes and xsamp. hs s realzed by qualfyng he poseror uncerany along he pah from n and xsamp usng he lnear covarance propagaon descrbed n Secon 7: gven he covarance Σ and he assocaed facored marx Ψ of n, he observaons z:samp and sae oupu x ˆ samp : are predced frs based on he closed-loop sysem model. Afer ha, he marx se S:samp, G:samp, N:samp and he ransfer funcon ς:samp along he pah are approxmaed. herefore, he poseror covarance Σ ˆ samp of xsamp resulng from movng he MAV along he pah beween n and xsamp can be propagaed by: ˆ ˆ 0 I 0 G Λ Λ samp Ψ samp = ς : sampψ = I N = G SG ˆ samp : Π samp : Π samp Σ ˆ =Λˆ Π ˆ 1 samp samp samp hence, he resulng uncerany cos of xsamp can be gven by x. ˆ ( ˆ samp Jsamp = r Σ samp ). he ree expanson process may yeld one or more canddae feasble nodes and correspondng rajecores. For each canddae node n and rajecory, he planner calculaes he pah cos (Lne 21), smulaes he observaon along he rajecory (Lne 22) and, hen, furher compues he poseror covarance a n, as well as he uncerany qualfcaon of n (Lne 23). Afer updang he oal cos, he algorhm fnally adds he feasble nodes o he curren ree (Lne 24) Pah Cos Evaluaon As aforemenoned n Algorhm 1, once a feasble node s denfed and added o he ree, he CL-RR algorhm evaluaes and updaes s assocaed cos (Lnes 21 24). In order o address he mulple requremens of operang n complex, GPS-dened envronmens, he CL-RB adops mulple cos mercs ha ncorporae varous facors no hese asks. In CL-RB, he cos of each node can be dvded no wo prmary caegores: he pah execuon cos and uncerany cos. he pah execuon cos denoes he cos of movng he MAV along he rajecory from he roo node nroo o he goal regon va he specfc node n, and can be gven as he followng form: Cp( n) = c[ x( ), u( )] d 0 where [0, ] s he me nerval of he correspondng pah (node sequence) {nroo n}, and c[x(), u()] s he cos merc of he node rajecory (.e., Eucldan dsance, Dubns dsance, ec.). In he CL-RB, he pah execuon cos consss of wo prmary componens: he accumulaed pah execuon cos and he cos-o-go: C ( n ) = C( n n ) + C p roo CosoGo

12 Sensors 2014, where C(n nroo) represens he accumulaed execuon cos resulng from followng he rajecory from nroo o n. here are wo ypes of cos esmaes of he cos-o-go for each node: a lower bound CLB and an upper bound CUB. he lower bound cos-o-go can be gven as he dsance merc beween he MAV s sae a he node and he goal sae: C ( n ) = C( x n ) LB goal he updae of he upper bound cos-o-go can be descrbed as follows: each me a new feasble node n s added o he ree, he CL-RB algorhm aemps o connec n o he goal by predcng he closed-loop rajecory beween n and xgoal. If he rajecory s feasble, he upper bound cos-o-go of n can be gven as he cos assocaed wh hs rajecory, oherwse he upper bound cos-o-go of n s se o : C xgoal UB = n j cx ( ( ), u ( )) : feasble rajecory beween n, x exss : no feasble rajecory beween n, x Afer ha, he CL-RB propagaes he pahs back owards he roo node o updae he upper bound cos-o-go of n s affeced ancesor nodes: he old upper bound cos-o-go of he paren node s compared wh he cos followng he newly generaed feasble rajecory. If he laer s smaller, he upper bound cos-o-go of he paren node s updaed; oherwse, he propagaon procedure sops, snce here exss a sub-pah of he paren wh a lower cos-o-go. hs updae procedure repeas unl he curren roo node s reached. he deals of he cos updae procedure are shown n Algorhm 2. 1 nc. LB cx ( goal n ) Algorhm 2. Closed-loop random belef ree: cos updae. 2 Compue he accumulaed cos of he rajecory beween n roo and n : Cn ( n roo ) 3 Generae he rajecory from n o goal sae x( xgoal n ), usng he closed-loop sysem model 4 Check he feasbly of he closed-loop rajecory x( xgoal n ) 5 f x( xgoal n ) s feasble hen 6 rajcoryogoal rue 7 nc. UB Cxx ( ( goal n )) (he pah cos of x( xgoal n )) n n 8 k 9 whle nk n and roo nk.paren. CUB > nk. CUB + C( nk.paren nk) 10 n.paren. k UB n k. UB + C ( k.paren k) 11 nk nk.paren 12 end whle nc. nc. 13 CosoGo UB 14 else 15 rajcoryogoal False 16 nc. UB 16 nc CosoGo 17 end f. nc. 18 nc. Cn ( nroo) + nc. CosoGo 19 reurn rajcoryogoal LB j j goal goal

13 Sensors 2014, Once a feasble rajecory o he goal s denfed, he upper bound cos-o-go s used as he esmaed cos-o-go of n; oherwse, he lower bound cos-o-go s used. herefore, he overall pah execuon cos of node n can be gven as: CUB : rajecoryogoal = rue Cn ( ) = Cn ( nroo ) + CCosoGo = Cn ( nroo ) + C : rajecoryogoal = False (18) (Noe ha he cos updae procedure also denfes feasble pahs ha connec he goal o he curren ree. hs enables he CL-RR o quckly fnd pahs ha reach he goal. As he ree grows, more pahs o he goal can be found). he CL-RB ncorporaes he qualfcaon of sae esmaon uncerany n he cos funcon of nodes, such ha he generaed pahs ensure accurae sae esmaon. As dscussed prevously, he sae esmaon uncerany s evaluaed usng he uncerany cos, e.g., he race of he predced covarance: J(n ) = r( Σ n ). As shown n Algorhm 1, once a feasble node s added o he ree and s pah execuon cos s updaed, he CL-RB predcs s poseror sae covarance hrough he updae seps descrbed n Secon 4 and compues s uncerany cos (Algorhm 1, Lnes 22, 23). akng he pah execuon cos and uncerany cos no consderaon, n CL-RB, he followng mul-objecve cos funcon s used: LB Cn ( ) = ζ 1Cn ( nroo ) + ζ 2CCosoGo + ζ3jn (. Σ ) (19) where CCosoGo denoes he esmaed cos-o-go from n o he goal sae, C(n nroo) s accumulaed along he pah from roo o n, and he frs wo ems correspond o he pah-execuon cos. J(n.Σ) s he cos assocaed wh he uncerany of node n, whch represens he sae uncerany resulng from followng pah {nroo n}. ζ1, ζ2, ζ3 are wegh facors for adjusng he relave mporance of fndng a shor duraon pah and reducng sae esmaon uncerany. Snce here s a unque pah from he roo o each of he nodes n he ree, he cos of a node can be used o represen he cos of he assocaed rajecory. he oal cos of he node s used o denfy he curren bes rajecory for execuon n he selecon and execuon procedure, whch s presened n he nex secon Pah Selecon and Execuon In order o accoun for he changes n me-varyng operang envronmens, he moon planner operaes by perodcally selecng and execung he bes poron of he curren rajecory whle connuously expandng he ree durng he execuon process. he pah selecon and execuon process s presened n Algorhm 3. Denoe he perod for he pah selecon and execuon as Δ. A he begnnng of each eraon perod, he algorhm updaes he curren acual sae of he MAV, as well as he envronmen nformaon. he purpose of hs sep s o ensure he MAV s suaonal awareness (Lne 4). Afer ha, he sae s frs propagaed o he end of he perod, resulng n x( 0 +Δ ) (Lne 4). hen, he curren roo of he ree s se o he mos curren node ha s followed by he propagaed sae, and all oher chldren nodes of he prevous roo are removed. hs s because he MAV s consdered o have passed he prevous roo, and he pahs of s chldren nodes wll never be execued. Durng each of he eraons, he planner denfes he bes poron of he rajecory n erms of he cos merc. Snce each node n n he ree s assocaed wh a sngle pah, he rajecory can be unquely specfed usng he sequence conssng of all of he

14 Sensors 2014, nodes from roo o n:{nroo n}. In CL-RB, he cos funcon gven n Equaon (19) s used for selecng he curren bes pahs. hs mul-objecve cos merc enables he moon planner o selec pahs ha acheve a balance beween fndng low-cos pahs o he goal and mnmzng localzaon uncerany. A he end of each pah selecon eraon sep, he seleced feasble rajecory s sen o he conroller o be execued, whle he ree keeps expandng (Algorhm 1) durng he remanng me of perod Δ. hs mechansm guaranees ha he pah plannng algorhm can provde a feasble pah for execuon n real me and accoun for uncerany n a dynamc envronmen. 1 0 Algorhm 3. Closed-loop random belef ree: execuon loop. 2 Inalze ree from node a x nal 3 whle x () Xgoal do 4 Updae he curren acual sae x( 0) and envronmen nformaon 5 Predc sae x() o x( 0 +Δ) and smulae observaons 6 whle me remanng < Δ do expand he ree usng Algorhm end whle Selec he curren bes feasble pah (node sequence) p { n n } 9 mulple-objecve cos funcon gven as Equaon (19) 10 f no bes feasble pah exss hen 11 Send brake command o he conroller and goo 3 12 else 13 Send p * o conroller o execue 14 end f 15 +Δ 16 end whle * 0... k he overall dagram of he CL-RB algorhm s depced n Fgure 2. = accordng o he Fgure 2. Dagram of he closed-loop random belef ree moon plannng framework. MAV, mcro-aeral vehcle. ΨΣ, n near r () u () x() { n... n} roo x sample J ( ) x sample x() C CosoGo cn ( n ) roo z () n near

15 Sensors 2014, Indoor Flgh Smulaon Resuls Based on a Smulaed Laser Scanner-Equpped MAV In order o valdae he effecveness of he CL-RB moon planner, we have conduced a number of expermens n varous smulaed scenaros ha are derved from real-world envronmens. he frs scenaro consss of a quadroor MAV navgang n a 3D wde-open ndoor envronmen, wh all srucures ha can be deeced by laser rangefnders concenraed along he perpheral walls. he frs scenaro s used o demonsrae he CL-RB s ably o generae he pah ha ensures he MAV s localzaon confdence. he subsequen scenaros nvolve more complex exensons, ncludng 3D unsrucured envronmens (he second scenaro), and he purpose of hese scenaros s o valdae he effecveness of he CL-RB algorhm n generang pahs ha sasfy mulple consrans mposed by dynamc, unsrucured envronmens and non-lnear MAV dynamcs, whle achevng a rade-off beween mnmzng pah cos and reducng localzaon uncerany n GPS-dened envronmens. For all of hese expermens, we assume ha he dynamcs model of he quadroor s non-lnear and ha he envronmen s whou access o GPS sgnals. he quadroor mus sar from an nal spo and raverse hrough he obsacles o a goal locaon, relyng on a pah-followng conrol law and he sae esmaes provded by an EKF esmaor. he exerocepve sensor equpped on he quadroor s assumed o be a smulaed laser rangefnder wh a maxmum sensng range of 2 m and a 240 feld-of-vew. For each of hese envronmens, a 3D model of he envronmen s frs generaed usng an RGB-D-based envronmenal modelng approach from our prevous work [26]. he 3D pon-cloud model s hen ransformed no a polyhedron-based model, whch s used for pah plannng. he CL-RB s mplemened n MALAB and all smulaons are performed n real me on an Inel 3.2 GHz plaform wh a 4 G RAM. he CL-RB algorhm selecs and execues he bes pars of he pahs every Δ seconds Quadroor Dynamcs Model he quadroor model ha s used n he smulaon s depced n Fgure 3. For he smulaon expermens, he x-y-z body-fxed coordnaes of he quadroor s derved usng he square confguraon, and he knemac and dynamc model of he quadroor s developed as rgd-body dynamcs nfluenced by he gravy and hrus of he roors, assumng near-hover aerodynamcs and neglecng he effecs caused by he ranslaonal velocy. he pure pursu reference law [27] s appled o seer he reference rajecory, and a PD conrol law s used o conrol he quadroor rackng of he reference. hs closed-loop sysem model conssng of he dynamcs model and he conrol law s used for boh he rajecory predcon of he CL-RB planner and he execuon of he resulan rajecory. Fgure 3. Srucure and confguraon of he quadroor MAV.

16 Sensors 2014, he confguraon of he quadroor ulzed n hs paper s llusraed n Fgure 3. wo coordnae sysems are consdered n our scheme: he navgaon frame (Earh-fxed frame) (xe, ye, ze) and he body-fxed frame (xb, yb, zb). he ranslaonal and roaonal moons of he quadroors are acheved hrough he forces and momens caused by varyng he angular raes of he four propellers. Assumng ha he quadroor srucure s rgd and symmercal and he cener of mass concdes wh he orgn of he body-fxed frame, he equaons of moon of he quadroor can be derved usng Newon s law and Euler equaons [27]. Defne φ, θ, ψ as he Euler angles denong he roll, pch and yaw aude, respecvely, he roaonal dynamcs can be descrbed as: Iyy Izz l Kdrx Jr ϕ = θψ + b( ω3 + ω4 ω1 ω2) ϕ+ θ( ω2 + ω4 ω1 ω3) Ixx Ixx Ixx Ixx I K zz I xx l dry Jr θ = ϕψ + b( ω2 + ω3 ω1 ω4) θ ϕ( ω2 + ω4 ω1 ω3) I yy Iyy Iyy Iyy Ixx Iyy ψ = ϕθ 2 + d( ω1 + ω3 ω4 Izz I ) Kdrz ω2 ψ zz I zz (20) where Ixx, Iyy, Izz are he momens of neral around he hree axs, Jr s he roor nera and l denoes he lengh of he momen arm. ω1, ω2, ω3, ω4 are he four propellers roaon speeds. b and d denoe he roor hrus coeffcen and roor drag coeffcen, respecvely. Kdrx, Kdry, Kdrz represen he roaonal drag coeffcens of he quadroor body. Defne (x, y, z) as he hree-dmensonal poson wh respec o he navgaon frame; he ranslaonal dynamcs of he quadroor can be gven as: 4 1 Kdx x = (cosϕcosψ snθ + snϕsn ψ) x m = 1 m 4 1 Kdy y = (snψ snθcosϕ snϕcos ψ) y m m 4 1 Kdz z = cosθcosϕ z g m m = 1 = 1 (21) where denoes he hrus generaed by propeller and = bω 2 1, m s he oal mass of he quadroor and g represens he gravaonal acceleraon. Kdrx, Kdry, Kdrz are he ranslaonal drag coeffcens of he quadroor body Conrol Scheme Desgn of he MAV Model Cascaded Conrol Scheme Under he assumpon of near-hoverng veloces, he drag erms caused by he ranslaonal and roaonal moons n Equaons (20) and (21) can be negleced, and he sysem dynamcs model can be descrbed n he followng sae-space form ( x = f (, x u) ):

17 Sensors 2014, ϕ θ ψ Iyy Izz l θψ + bu 2 Ixx I xx Izz I xx l ϕψ ++ bu3 I yy Iyy Ixx Iyy 1 ϕθ du4 x = + Izz I zz x y z 1 (cosϕcosψ snθ + snϕsn ψ) u 1 m 1 (snψ snθ cosϕ snϕcos ψ) u 1 m 1 cosθ cosϕ u1 g m (22) where x = ϕ θ ψ ϕ θ ψ x y z x y z represens he conrol npu: denoes he sae vecor, and u = [ u u u u ] b( ω1 + ω2 + ω3 + ω4) b( ω3 + ω4 ω1 ω2) u = b( ω2 + ω3 ω1 ω4) (23) d( ω1 + ω3 ω4 ω2) he physcal meanng of he conrol npu s relaed o he forces generaed by he four propellers: u1 represens he oal hrus of he propellers, whle u2, u3 and u4 are he dfferences of he propeller pars. As can be seen from Equaon (20), he roaonal dynamcs (φ, θ, ψ) are ndependen of he ranslaonal dynamcs (x, y, z), leadng o decoupled sysem dynamcs. In addon, he alude dynamcs (z) can also be decoupled from he planar ranslaonal dynamcs (x, y) by assumng ha he φ, θ angles are very small. As a resul, he quadroor dynamcs n Equaon (21) can be decoupled no ndependen sub-sysem dynamcs, and he cascaded conrol sysems for he aude, poson and alude can be desgned from he decoupled dynamcs. Under he assumpon of small φ, θ angles and near-hoverng moons, we can derve he followng lnearzed aude dynamcs: l ϕ = bu I x l θ = bu I y d ψ = u 4 I z 2 3 (24) he lnearzed ranslaonal moon dynamcs can be gven as: 1 x = ( θcosψ + ϕsn ψ) u1 m 1 y = ( θsnψ ϕcos ψ) u1 m 1 z = u1 g m (25)

18 Sensors 2014, Usng he decoupled roaonal and ranslaonal dynamcs n Equaons (24) and (25), we can desgn a cascaded conrol scheme conssng of he nner conrol loop and ouer conrol loop, of whch he conrol laws can be desgned separaely. As llusraed n Fgure 4, he ouer conrol loop s he poson and alude conroller, whch receves he reference pah from he CL-RB planner and generaes he reference roll and pch commands (φr, θr) o he nner aude conrol loop, whle he aude conroller produces he desred roor speed o acheve he reference aude accordng o he aude commands φr, θr, ψr. Fgure 4. Cascaded conrol scheme of he quadroor MAV. CL-RB, closed-loop random belef rees. x, y r r ϕ, θ r r ψ r u2, u3, u4 x z r x, y u 1 ϕ, θ, ψ y z ψ z Conroller Desgn Based on he aude and ranslaonal moon dynamcs descrbed n Equaons (24) and (25), he proporonal-negral-dervave (PID) conrol scheme s ulzed o desgn he aude and poson conroller for he quadroor MAV. he frs sep s o consder he nner loop conroller, whch funcons as he core of he conrol scheme. Usng he aude dynamcs shown n Equaon (24), he aude conroller can be desgned as follows: ϕθ, ϕθ, ϕθ, u2 = KP ( ϕr ϕ) + KI ( ϕr ϕ) d+ KD ( ϕr ϕ) u = K ( θ θ) + K ( θ θ) d+ K ( θ θ) 3 4 ϕθ, ϕθ, ϕθ, P r I r D r ψ ψ ψ u = K ( ψ ψ) + K ( ψ ψ) d+ K ( ψ ψ) P r I r D r ϕ, θ ϕ, θ ϕ, θ ψ ψ where KP, KI, KD, KP, KI, K ψ D are he conrol gans of he PID conroller and φr, θr and ψr denoe he reference roll, pch and yaw, respecvely. Noe ha φr, θr are generaed by he ouer conrol loop whle ψr s drecly provded by he CL-RB planner. Under he assumpon of near-hoverng veloces and small roaonal angle moons, he conrol scheme of ranslaonal dynamcs can be decoupled no he planar (x, y) and alude conrollers ha can be desgned separaely. he alude conroller s desgned based on PID conrol wh non-lnear compensaon: 1 z z z u1 = ( KP( zr z) + KI ( zr z) d+ KD( zr z)) cosθcosϕ z z z where KP, KI, K D are he PID conrol gans, and zr s he reference alude of he pah provded by he CL-RB planner. he planar poson conrollers ake he followng form: (26) (27)

19 Sensors 2014, x x x θ = cos ψ[ K ( x x) + K ( x x) d+ K ( x x)] r P r I r D r y y y + sn ψ [ K ( y y) + K ( y y) d+ K ( y y)] P r I r D r x x x φ = sn ψ[ K ( x x) + K ( x x) d+ K ( x x)] r P r I r D r y y y cos ψ [ K ( y y) + K ( y y) d+ K ( y y)] P r I r D r x x x y y y where KP, KI, KD, KP, KI, K D are he PID conrol gans and xr, yr are he reference poson provded by he CL-RB planner. As menoned before, he ouer loop conroller oupus θr, φr o he nner loop conroller as he reference ph and roll, whch are used n Equaon (26) for aude conrol. In our applcaons, he frequency of he nner loop (aude conroller) s 250 Hz, whle he ouer loop (ranslaonal and alude conroller) runs a 30 Hz. he PID gans of he conrollers are uned hrough exensve numercal smulaons n order o acheve a desrable conrol performance. In hs paper, we only repor smulaon resuls based on he PID conrol law. he reason for selecng he PID scheme s ha can handle complex dynamcs model and s robus o modelng errors. Noe ha more complex and advanced conrol laws can also be adoped n he CL-RB framework. In addon, oher ypes of MAV dynamcs models wh assocaed gudance and conrol laws can be ncorporaed o form he closed-loop model n CL-RB EKF Process Model In he smulaon expermens, he saes of he MAV are esmaed usng an EKF-based esmaor. he MAV saes o be esmaed n he EKF fler nclude: hree-axs poson n he global frame p = [x, y, z], MAV orenaon represened usng Euler angles [φ, θ, ψ] (roll, pch and yaw), hree-axs velocy n he global frame v = [vx, vy, vz], as well as he gyroscope bas bω = [bωx, bωy, bωz] and acceleromeer bas bf = [bfx, bfy, bfz]. he esmaed sae vecor can be denoed specfcally as follows: x= [ p, v, ϕ, θ, ψ, bω, b f] = [ x, yzv,, x, vy, vz, ϕ, θ, ψ, bω x, bω y, bω z, bfx, bfy, bfz] (28) As descrbed prevously, he MAV s equpped wh a laser rangefnder, whch s used by scan-machng o provde poson and headng measuremens (x, y, ψ). he alude of he MAV s measured by an onboard sonar almeer, and s assumed ha he alude measuremen s no affeced by he aude of he MAV. In addon o he laser rangefnder, he MAV s assumed o be equpped wh an IMU module, whch consss of a raxal gyroscope and raxal acceleromeer. he raxal gyroscope provdes hree-axs angular raes ωm = [ωmx, ωmy, ωmz], whle he acceleromeer measures he hree-axs acceleraons fm = [fmx, fmy, fmz], and he above IMU measuremens are all expressed n he MAV s body frame. Usng he above defnons, he connuous sae-space-based process model (x= g(x, u)) descrbng he IMU dynamcs can be gven by: p = v (29) n v = C b( fm b f ) g (30) φ 1 sn φan θ cos φan θωmx bω x θ 0 cos sn my b = φ φ ω ωy ψ 0 sn φsecθ cos φsec θ ωmz b ωz (31)

20 Sensors 2014, b = 0 ω (32) b f = 0 (33) n where C b denoes he ransformaon marx from he body frame o he global frame, and g = [0, 0, g] s he local gravy vecor n he global frame. Snce he x, y poson and he headng ψ (yaw) of he MAV are esmaed separaely usng he measuremens from he laser scan-machng, ψ can be reaed ndependen of roll φ and pch θ, and he MAV moon of he x-, y-axes can be decoupled from z-axs moon under he assumpon of near-hoverng velocy and small φ, θ angles. hs assumpon yelds he smplfed ransformaon marx n Equaon (30): n C b cos ψ sn ψ 0 = sn cos 0 ψ ψ A smlar approach for he smplfcaon of he MAV moon can also be found n [28,29] (Noe ha alhough hs smplfed velocy model may cause sgnfcanly large esmaon error when he MAV performs aggressve aude maneuvers or hgh-speed flgh, s suffcenly accurae for sae esmaon under he above assumpons n hs paper). o mplemen he sae esmaon on a compuer sysem, he above process model s ransformed no a dscree-me model. Le Δ be he updae perod of EKF and assume ha Δ s suffcenly small, he process model n Equaons (29) (33) can be dscrezed as follows: (34) x = x +Δf ( x, u ) (35) In order o mplemen he EKF esmaor, he above dscree process model needs o be furher lnearzed by calculang he Jacobans g / x x as descrbed n Equaon (2). o derve he Jacobans, he calculaon 1 of paral dervaves of he process model wh respec o he sae vecor s gven as follows: I33 ΔI I33 ΔG ΔG 25 g f G = = I +Δ = I33 + ΔG33 ΔG x x x 1 x I I 33 (36) where: 0 0 sn ψ( fmx bfx ) cos ψ( fmy bfy ) G23 = 0 0 cos ( fmx bfx ) sn ( fmy bfy ) ψ ψ 0 0 0, G25 cos ψ sn ψ 0 = sn cos 0 ψ ψ ( ωmy bω y )cos ϕan θ ( ωmz bω z )sn ϕan θ ( ωmy bωy )sn ϕsec θ+ ( ωmz bωz ) cos ϕsec θ 0 G33 = ( ωmy bωy )sn ϕ ( ωmz bωz )cosϕ ( ωmy bωy )cos ϕsec θ ( ωmz bω z )sn ϕsec θ ( ωmy bωy )sn ϕsn θsec θ+ ( ωmz bωz ) cos ϕsn θsec θ 0 1 sn ϕ an θ cos ϕ an θ G34 = 0 cos sn ϕ ϕ 0 sn ϕsecθ cos ϕsecθ

21 Sensors 2014, he process nose w s nroduced by he IMU angular raes (ωm) and acceleraons (fω), whch are ncluded as he npu u o he process model: [ ] u = ω m, fm,, w= wω, w f (37) where wω, wf are he nose assocaed wh he angular raes and acceleraons, respecvely, and wω, wf are boh whe Gaussan noses wh zero mean,.e., wω~n(0, Qω), wf~n(0, Qf). Smlarly, he paral dervaves of he process model wh respec o he process nose can be calculaed by: V g f ΔV = =Δ = w 1, w 1 1, V x u x u Δ 33 (38) where: 1 sn ϕ an θ cosϕ an θ n V22 = C b, V31 = 0 cos sn ϕ ϕ 0 sn ϕsec θ cos ϕsecθ Due o he dfferen measuremen raes of he IMU, he sonar and he scan-machng module, he sae and covarance are updaed separaely a dfferen mes, usng hree groups of measuremens. he pch, roll angle (φ, θ) and he gyroscope bas bω are updaed frs usng he measuremens from he IMU (more specfcally, he acceleromeer measuremens). he alude z s updaed separaely usng he sonar measuremen, snce s uncorrelaed wh he oher measuremens. In addon, he updae of he MAV poson p, headng ψ, he velocy vx, vy and he acceleromeer bas bf are compleed when he measuremens from laser scan-machng are receved. By he above dscussons, whle he performance of pch and roll (φ, θ) esmaes depends on he IMU, he esmaon of x, y poson and he headng ψ are prmarly affeced by he characerscs of he laser rangefnder, as well as he perceved envronmen nformaon. As a resul, n order o evaluae how he pah plannng sraegy affecs he uncerany of sae esmaon, we exrac he laser-relaed saes x = [ xyv,, x, vy, ψ, bfx, bfy ] from he full sae vecor. By Equaon (35), he process updae model of x can be gven as: x = x 1 + vx 1Δ y = y 1 + vy 1Δ vx vx cos ψ sn ψ fmx bfx cos ψ sn ψ wfx v = y v +Δ y sn cos f sn cos 1 1 my b +Δ fy w ψ ψ 1 ψ ψ 1 fy (39) ψ =ψ 1 +Δ( ωmz 1 bω z 1) +Δwω z bfx = bfx 1 b = bfy 1 fy By he above process model, we have: x = g( x 1, u 1, w ) (40) whereu = fmx, fmy, ω mz, w = wfx, wfy, w ω z drecly from Equaons (36) and (38):, and he Jacobans of he above model can be obaned

22 Sensors 2014, Δ Δ Δsn ψ( fmx bfx ) Δcos ψ( fmy bfy ) Δcosψ Δsnψ g G = = Δcos ψ( fmx bfx ) Δsn ψ( fmy bfy ) Δsnψ Δcosψ x x 1u (41) 0 0 Δcosψ Δsnψ V = 0 0 sn cos Δ ψ Δ ψ (42) Δ 0 0 Denoe σ 2 fx, σ 2 fy and σ 2 ωz as he nose covarance of he acceleromeer measuremens n he x- and y-axs, as well as he gyroscope measuremens n he z-axs, respecvely. he process nose covarance of he process model n Equaon (39) can be gven by: Q dag ω herefore, he marx S n Equaon (11) can be calculaed by = ( σfx, σfy, σ z) (43) S = VQV, and G (Equaon (41)), V (Equaon (42)), S are used n he lnear covarance propagaon, as descrbed n Equaon (11) Sensor Measuremen Model and Uncerany Analyss of Laser Rangefnders In many acual applcaons, a MAV performs localzaon usng an onboard laser rangefnder, whch obans measuremens hrough a seres of range readngs recorded a consecuve angles ncremens. herefore, he accuracy of he localzaon s manly affeced by he range o he objecs and he opology of he envronmens. In our smulaon, we assume ha he quadroor s equpped wh a laser rangefnder and ha he localzaon s performed by machng he laser scans. In order o ncorporae he uncerany of he laser rangefnder for he covarance propagaon n he CL-RB planner, we used an nformaon marx-based mehod [6] o deermne he uncerany of he laser measuremens wh respec o he opology of he envronmens. For a quadroor usng a laser rangefnder, he nformaon marx N n Equaon (11) denoes he nformaon provded by laser scans, whch can be calculaed by combnng he nformaon of each range measuremen of he envronmen. Consderng an envronmen geomery shown n Fgure 5, a laser scan obaned a me consss of measuremens of n scan pons n he envronmen, each of whch s descrbed by a range readng r and a measuremen angle θ, and he nformaon provded n a laser scan (r, θ) s n he drecon perpendcular o he opology,.e., he drecon of he vecor normal (he vecor n green, Fgure 5) of he envronmen surface. herefore, he nformaon marx can be obaned by projecng he range nformaon ono he vecor normal of he surface and summng he projeced nformaon of each scan pon. he nformaon marx N can be calculaed by: (44)

23 Sensors 2014, wh H as he measuremen marx (Equaon (45)) and Σ 2 r as he varance marx (Equaon (46)) denong he varance of each measuremen: cosγ1cos( γ1 θ1) snγ1cos( γ1 θ1) r1sn( γ1 θ1) H = (45) cosγncos( γn θn) cosγncos( γn θn) rsn( γ n θn) Σ =dag( σ, σ, σ,... σ, σ, σ... σ, σ, σ ) r r1 r1 r1 r r r rn rn rn In our expermens, he readngs of each measuremen pon (r, θ) are obaned frs from he smulaed laser scan, and hen, he lne segmens of he opology surface are exraced usng he measuremen pons, along wh he vecor normal of each lne segmens, as well as he angle beween he body axs and he vecor normal. From Equaons (44) (46), he nformaon marx N can be calculaed wh he summaon erms ha are gven as follows: xx ψψ m 2 2 m 2 2 cos γcos ( γ θ) sn γcos ( γ θ), 2 yy 2 σr σ r m 2 2 m 2 r sn ( γ θ) cosγsnγcos ( γ θ), 2 xy 2 σr σ r m m r r, 2 yψ 2 σr σ r Σ = Σ = Σ = Σ = cosγ sn( γ θ )cos( γ θ ) sn γ sn( γ θ )cos( γ θ ) Σ = Σ = xψ (46) Fgure 5. Uncerany analyss of a laser range measuremen. he blue pons denoe he measuremen pons n a sngle laser scan a me. γ s he angle beween he body axs and he vecor normal of he surface a measuremen pon (x, y), and β s he angle form he vecor normal o he measuremen drecon. d s he dsance from he orgn o he lne segmen. x b θ γ β r σ r x, y σ r d y b he above analyss provdes a sascal approach o ncorporang he varance of he laser scan daa no he uncerany of he pose esmae. herefore, we can use Equaons (44) (46) o calculae he nformaon marx N, whch s used n he lnear covarance propagaon as descrbed n Equaon (11). Fgure 6a e show he uncerany ellpses relaed o he poson esmaon unceranes along a predefned pah calculaed usng he nformaon marx equaons proposed n hs secon, as well as

24 Sensors 2014, he covarance propagaon approaches n Secon 4. In he expermens, an 18 9 m 3D envronmen model s derved from a cluered real-world laboraory, and he MAV s assumed o be equpped wh a smulaed laser rangefnder ha s able o generae range and bearng measuremens of he envronmen. In order o hghlgh he nfluences of he envronmen, he laser sensor model s assumed o have a lmed sensng capably wh a 2-m range and a 240 feld-of-vew, whch s represened by blue secors n he fgures (Alhough hs sensor model s unrealsc n erms of maxmum range, serves well o llusrae how he envronmen and sensor measuremen affec he localzaon uncerany). As he MAV navgaes along he pah and roaes s orenaons, he envronmen opologes ha fall whn he feld-of-vew wll generae a seres of nosy measuremens of he opologes dsance and bearng o he MAV body (he vald measuremens are demonsraed by he cyan secors n he followng fgures), enablng he MAV o localze self by a EKF-based approach usng hese measuremens of he envronmen. he 1 σ poson uncerany s demonsraed by red ellpses n he fgures, and he sze and shape of he ellpses ndcae he relave scale of he localzaon uncerany n boh x and y drecons. Fgure 6. Poson esmaon unceranes based on a smulaed laser sensor along a predefned pah. Cyan secors denoe he laser daa wh vald envronmenal measuremens. Red ellpses llusrae he 1 σ localzaon covarance; larger ellpses ndcae hgh uncerany poses. All covarance are n cm. (a) poson: (5.0, 2.0, 0.5) m, headng: 45 ; (b) poson: (8.0, 7.5, 0.5) m, headng: 45 ; (c) poson: (8.0, 7.5, 0.5) m, headng: 0 ; (d) poson: (11.0, 4.0, 1.0) m, headng: 30 ; (e) poson: (10.0, 2.5, 1.0) m, headng: 0. (a) (b) (c) (d)

25 Sensors 2014, Fgure 6. Con. (e) As can be seen from Fgure 6, he area where he onboard sensor s expeced o deec more envronmenal opologes ends o produce poson esmaes wh low uncerany (Fgure 6a,b), whle hose locaons where he sensor only encouners jus one obsacle wll lead o hgh localzaon uncerany (Fgure 6d). In exreme cases where he MAV navgaes no wde open areas (Fgure 6e), he localzaon uncerany ncreases sharply, snce hese areas provde almos no nformaon for localzaon. In addon, he localzaon uncerany s also affeced by he orenaon (yaw) of he MAV relave o he envronmenal opologes: he MAV s a he same x-y-z coordnaes n Fgure 6b,c, bu he poson uncerany vares due o he dfferen yaw orenaons of he sensor ( 45 n Fgure 6b and 0 n Fgure 6c); he yaw angle s posve when he roaon s counerclockwse around he z-axs of he Earh-fxed frame). he envronmenal opology of he MAV locaon n Fgure 6b,c can be consdered as a corrdor beween wo parallel obsacles, and he laser sensor measuremens wll provde mos nformaon (large nformaon marx N) n he drecon perpendcular o he local envronmen, accordng o he model gven n Equaons (44) (46). As a resul, he uncerany n he drecon of he corrdor (x drecon of he envronmen frame) s much larger when he measuremen drecon s perpendcular o he corrdor (Fgure 6c) Numercal Smulaon Resuls Scenaro 1: Indoor Envronmen wh Open Space he envronmen of he frs scenaro s shown n Fgure 7a. As can be seen from Fgure 7, hs scenaro s a m open ndoor envronmen wh all srucures concenraed along he perpheral walls, and he cener of he envronmen s an open regon ha s ou of he measuremen range of he smulaed laser rangefnder. he MAV quadroor mus navgae from he nal poson a he boom-rgh corner hrough he hallway o a goal regon, whch s dagonally oppose of he nal pon. Snce mos of he envronmen provdes rare measuremen for localzaon, s even more crcal for he pah plannng sraegy o fnd pahs ha maxmze nformaon gan and reduce sae uncerany; hence, he emphass of hs smulaon scenaro s o es he pah plannng algorhm s ably o generae pahs ha ensure he MAV s localzaon performance hroughou he pah. For comparson purposes,

26 Sensors 2014, he convenonal CL-RR algorhm whou he ncorporaon of sae uncerany s also esed usng he same MAV model and envronmen confguraons. Fgure 7. Indoor envronmen smulaon Scenaro 1: (a) A hallway of snghua Unversy s man buldng; (b) smulaed 3D model of he GPS-dened ndoor envronmen. he red and blue crcles ndcae he nal and goal locaon of he MAV, respecvely. (a) (b) In hs scenaro, he quadroor MAV begns a x [ ] nal = m. I s nended o navgae owards he goal locaon a x [ ] goal = m, and he wegh facors n Equaon (19) are se as: ζ1 = 1, ζ1 = 1, ζ3 = 100. he cascaded PID conrol scheme descrbed n Secon 6.2 s appled for boh CL-RB sae propagaon and reference pah followng of he MAV. he plannng-execuon cycle s se o 5 s. Fgure 8 demonsraes an example rajecory generaed by a ral of he smulaon scenaro. he CL-RB algorhm quckly generaes a feasble pah ha goes hrough he regons wh as much measuremens as possble o localze and reaches he goal (Fgure 8a,b). Snce a comparavely small amoun of samples and nodes are generaed durng he nal plannng, he nally seleced pahs may sll reach he open regons wh comparavely hgh localzaon uncerany (Fgure 8c e). However, as more samples and nodes are added, he CL-RB connuously refnes he pah n real me, denfyng opmal pahs ha go hrough measuremen-rch regons o ensure localzaon and reduce he execuon cos of he goal (Fgure 8f h). In conras, by expandng he pah usng he convenonal CL-RR and checkng obsacle collson consrans, he resulng pahs wll move he MAV sragh o he goal (Fgure 9). Alhough he convenonal CL-RR may generae shorer pahs, gnorng he localzaon facors wll resul n hgh localzaon uncerany, snce hese pahs may go hrough he open regons. In hs case, followng hese pahs would lkely cause operaon falure, snce he sae esmae becomes unaccepably unceran, such ha he MAV conrol would have become unsable.

27 Sensors 2014, Fgure 8. Example resuls of rajecory generaed by CL-RB for a quadroor MAV navgang n smulaon Scenaro 1. he pnk ellpses represen he covarance of he poson esmae, and large ellpses denoe MAV saes wh hgh uncerany. he curren sae (MAV s 3D posons and headngs) s denoed wh a red penacle. he curren seleced pah (specfed by node sequence, used as he reference o he conrol sysem) s marked n black wh red dos represenng he nodes. he predced closed-loop rajecory correspondng o he curren seleced node sequence s emphaszed n red, whle he acual oupu rajecory flown by he closed-loop MAV model s marked n pnk. he curren ree s denoed by green nodes and cyan rajecores, bu s se as nvsble afer (b) for clary. (a) = 5 s; (b) = 10 s; (c) = 20 s; (d) = 25 s; (e) = 30 s; (f) = 40 s; (g) = 50 s; (h) = 60 s. (a) (b) (c) (d) (e) (f)

28 Sensors 2014, Fgure 8. Con. (g) (h) Fgure 9. Example resuls of he rajecory generaed by he convenonal closed-loop rapdly explorng random rees (CL-RR) for a quadroor MAV navgang n smulaon Scenaro 1. he red cross n (b) denoes he sae where he MAV s sae esmaon fals, snce he poson uncerany has become suffcenly hgh. (a) = 5 s; (b) = 25 s. (a) (b) Scenaro 2: Cluered 3D Indoor Envronmen hs scenaro consders a more complex hree-dmensonal, GPS-dened ndoor envronmen wh a number of non-convex obsacles and srucures (Fgure 10). he envronmen model s derved from an acual cluered laboraory, whch s he same as he model shown n Fgure 6. he objecve of he quadroor MAV s o navgae from he boom-lef nal pon o he goal poson locaed on he oher sde of he laboraory, movng safely beween he obsacles. Plannng rajecores n hs cluered envronmen s a challengng ask for he pah planner. he unsrucured envronmen requres he pah plannng algorhm o be able o generae a rajecory ha avods he non-convex obsacles o ensure feasbly, whle boundng he sae esmae uncerany along he rajecory o allow he MAV o reman well-localzed.

29 Sensors 2014, Fgure 10. Indoor envronmen smulaon Scenaro 2: (a) vew of a ypcal cluered laboraory; (b) smulaed 3D model of he unsrucured, GPS-dened ndoor envronmen. he red and blue do ndcae he nal and goal locaon of he MAV, respecvely. (a) (b) For hs scenaro, he quadroor MAV begns from one sde of he envronmen a x [ ] nal = m, and s nended o navgae o he goal pon behnd a cluered regon of obsacles a x [ ] goal = m. he same MAV dynamcs model, PID conrol scheme and sensor confguraon as n Scenaro 1 are also appled n hs scenaro. However, we selec weghng facors n Equaon (19) as ζ1 = 1, ζ1 = 20, ζ3 = 30, snce he emphass of hs scenaro s on generang feasble rajecores ha quckly reach he goal whle reducng localzaon uncerany. he plannng cycle n hs scenaro s exended o 7 s. he convenonal CL-RR s also esed usng he same scenaro and MAV sysem model for comparson purpose. An example of he rajecory smulaon resuls from hs scenaro s depced n Fgure 11. As can be seen from Fgure 11, he MAV s nal poson s locaed n an obsacle-rch regon. he pah planner nally has o move he MAV ou of hs cluered regon. Unlke he frs scenaro, he CL-RB algorhm does no fnd any rajecores drecly o he goal due o he poor coverage of he CL-RB ree and he cluered envronmen n he frs few plannng cycles (Fgure 11a,b). As he CL-RB ree expands o cover more space afer some wanderng, he CL-RB algorhm moves he MAV progressvely hrough he narrow passages and approaches owards he drecon of he goal regon Fgure 11c). Afer movng ou of he narrow passage regon, he pah planner succeeds n denfyng a rajecory o he goal (Fgure 11d). As can be seen from he fgure, he covarance of he poson esmae says bounded along he rajecory durng he enre plannng cycles of CL-RB. In conras o CL-RB, he convenonal CL-RR quckly fnds a pah ha goes over he cluered obsacle regon and drecly reaches he goal (Fgure 12a). Alhough he resulng pah s shorer han he pah generaed by CL-RB, he uncerany of he poson esmae grows rapdly along he CL-RR rajecory (Fgure 12b); hs s because he envronmen becomes open as he MAV s alude ncreases, provdng rare nformaon o he onboard sensor for localzaon.

30 Sensors 2014, Fgure 11. Example resuls of he rajecory generaed by he CL-RB for a quadroor MAV navgang n a cluered envronmen. he full legend for he symbols can be found n Fgure 8. he curren ree s se as nvsble afer (b) for clary. (a) = 7 s; (b) = 14 s; (c) = 21 s; (d) = 28 s; (e) = 35 s; (f) = 42 s; (g) = 56 s; (h) = 70 s. (a) (b) (c) (d) (e) (f)

31 Sensors 2014, Fgure 11. Con. (g) (h) Fgure 12. Example resuls of rajecory generaed by he convenonal CL-RR for a quadroor MAV navgang n smulaon Scenaro 1. he red cross n (b) denoes he poson where he MAV s sae esmaon fals snce he poson uncerany have exceeded he hreshold. (a) = 7 s; (b) = 28 s. (a) (b) Performance Comparson and Analyss In order o llusrae he performance of he CL-RB algorhm, we compare he performance sascs of he CL-RB algorhm o ha of he convenonal CL-RR by runnng smulaon expermens usng boh algorhms on he same scenaros, as descrbed n Secons and (noe ha we dd no compare he CL-RB o oher convenonal samplng-based pah planners ha operae on sac graphs or rees, such as PRM, RR or BRM, snce hey are no real-me algorhms n essence). For each smulaon scenaro, he expermen s repeaed 30 mes usng each of he wo algorhms, and he performance sascs s recorded and evaluaed n erms of he pahs cos (oal lengh) and he race of he MAV s expeced covarance a he goal when followng he generaed pahs, boh averaged over 30 mes.