SAFE beyond Visual Line-Of-Sight (VLOS) operations

Transcription

1 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED NOVEMBER, There s No Place Like Home: Visual Teach and Repea for Emergency Reurn of Muliroor UAVs During GPS Failure Michael Warren, Melissa Greeff, Bhavi Pael, Jack Collier, Angela P. Schoellig, and Timohy D. Barfoo Absrac Redundan navigaion sysems are criical for safe operaion of UAVs in high-risk environmens. Since mos commercial UAVs almos wholly rely on GPS, jamming, inerference and muli-pahing are real concerns ha usually limi heir operaions o low-risk environmens and VLOS. This paper presens a vision-based roue-following sysem for he auonomous, safe reurn of UAVs under primary navigaion failure such as GPS jamming. Using a Visual Teach and Repea framework o build a visual map of he environmen during an oubound fligh, we show he auonomous reurn of he UAV by visually localising he live view o his map when a simulaed GPS failure occurs, conrolling he o follow he safe oubound pah back o he launch poin. Using gimbal-sabilised sereo vision and inerial sensing alone, wihou reliance on exernal infrasrucure, Visual Odomery and localisaion are achieved a aliudes of 5-25 m and fligh speeds up o 55 km/h. We examine he performance of he visual localisaion algorihm under a variey of condiions and also demonsrae closed-loop auonomy along a complicaed 45 m pah. Index Terms Aerial Sysems: Percepion and Auonomy, Visual-Based Navigaion, Sensor-based Conrol I. INTRODUCTION SAFE beyond Visual Line-Of-Sigh (VLOS) operaions are criical o enhancing he uiliy of Unmanned Aerial Vehicles (UAVs) in large-scale, oudoor operaions. Typically, reliance on Global Navigaion Saellie Sysems (GNSS) for navigaion in mos low-cos commercial UAVs mean he auhorisaion o do so from governmen regulaors is rare. Jamming, inerference and accuracy concerns mean ha Global Posiioning Sysem (GPS) alone canno be relied on in cases of close-proximiy, safey-criical or high-value operaions. In his paper, we presen a complee vision-only rouefollowing sysem for he auonomous navigaion of UAVs, and demonsrae is use as a funcional backup sysem for GPS-only navigaion. Using his sysem allows he o navigae home visually in case of primary navigaion sysem failure, wihou reliance on any exernal infrasrucure, or inerial sensing for he vision-based componens. Manuscrip received: Sepember ; Acceped November 5, 218. This paper was recommended for publicaion by Edior Jonahan Robers upon evaluaion of he Associae Edior and Reviewers commens. This work was suppored by he Smar Compuing for Innovaion Consorium (SOSCIP), Defence Research and Developmen Canada (DRDC), Drone Delivery Canada (DDC), Naural Sciences and Engineering Research Council of Canada (NSERC) and he Cenre for Aerial Roboics Research and Educaion (CARRE), Universiy of Torono. Jack Collier is wih Defence Research and Developmen Canada: Jack.Collier@drdc-rddc.gc.ca. All oher auhors are wih he Universiy of Torono Insiue for Aerospace Sudies (UTIAS), Canada: {michaelwarren, melissa.greeff, bhavi.pael}@roboics.uias.uorono.ca, {angela.schoellig, im.barfoo}@uorono.ca. Accompanying video a: iny.cc/noplacelikehome. Digial Objec Idenifier (DOI): see op of his page. 73cm Fig. 1: The experimenal seup for VT&R on our muliroor UAV: (1) DJI Marice 6 Pro plaform, (2) DJI A3 riple-redundan GPS module, (3) DJI Ronin-MX 3-axis gimbal, (4) NVIDIA Tegra TX2, (5) SereoLabs ZED. Visual Teach and Repea (VT&R) is a pah-following algorihm capable of auonomously driving a robo by following a previously raversed roue [1]. Using visual feaure maches from a live view o a locally meric map of 3D poins allows he robo o esimae a pah offse and send correcions o a pah-following conroller [2]. Tradiionally, VT&R is used on wheeled s [3], wih applicaions over consrained pahs where exernal navigaion infrasrucure is unreliable or no available, e.g., facory floors, orchards, mines, urban road neworks, and exploraory search-and-reurn missions. Using VT&R on aerial plaforms has a number of unique use cases: jus-in-ime deliveries beween warehouses, where fligh pahs are generally resriced o a few, high-frequency roues; monioring of sensiive asses such as propery borders or high-value infrasrucure; and auonomous parol in closeproximiy environmens, where poor sky view and jamming are noable concerns. Significanly, we wan he o be able o auonomously and safely reurn o he ake-off locaion a any ime by using vision o localise o a map generaed during he oubound pah, all during a single fligh. In his paper, we adap he radiional VT&R mehodology o sui hese arge use cases and apply our VT&R 2. sysem [3] on-board a muliroor UAV (Fig. 1) o demonsrae closed-loop operaion. We show resuls of live localisaion a speeds up o 15 m/s (55 km/h) a low aliude (5-25 meres) in winds up o 8 m/s, and demonsrae vision-based pah-following conrol for he reurn segmen of a jus-augh oubound pah. The novel work of his paper includes 1) demonsraion of he VT&R framework on a new plaform, a UAV wih gimbal-sabilised, 2) a horough analysis

2 2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED NOVEMBER, 218 of localisaion performance in his new scenario and 3) presenaion of a pah-following conroller for muliroor UAVs applied specifically in a VT&R framework, all in a wide range of oudoor es scenarios. The res of his paper is oulined as follows: Secion II examines similar work in visual roue following for ground s and UAVs, and explores recen work in auonomous vision-based navigaion of UAVs. Secion III describes he VT&R mehodology for applicaion on our arge UAV, including he VT&R framework, localisaion algorihm and gimbal and conrollers. Secion IV describes he experimenal seup o es he airborne VT&R framework, as well as descripion of daases, field ess and resuls. The paper is concluded in Secion V. II. PREVIOUS WORK VT&R and similar roue-based navigaion algorihms have a rich hisory on ground plaforms [1], [2], [4], [5], wih he mos recen exension adaped o include muliple experiences, increasing he auonomous performance ime from a few days o several monhs [3]. On UAVs, here are now several demonsraions of each-and-repea syle algorihms from he auhors of his paper and ohers [6] [9]. Our previous work, demonsraing he localisaion performance of VT&R on fixed-wing UAVs [7] and inegraion of a gimbal-sabilised on a ground [1], is he leadup o his work. While here are few examples using a gimbalsabilised on ground s, a number of examples exis in demonsraions on UAVs [11] [15]. This discrepency can mos likely be aribued o he larger dynamic moions of UAVs, where he uiliy of a gimbal is highly jusified o ensure smooh sensor moion. In all he above cases, however, only wo-axis gimbals are uilised. In our seup, we use an off-he-shelf hree-axis gimbal o aenuae moion in all hree roaional axes. The approach ha is closes concepually o our work, wih specific applicaion on UAVs, is [9]. Despie no being framed as a each-and-repea echnique, his sysem presens a demonsraion of such a mehod on a UAV using a visualinerial framework wih weak GPS priors o assis iniialisaion of localisaion and inform loop closures. Our work differs in ha i requires no offline map building (he map is buil on-board in real ime) and does no require inerial sensors or exernal infrasrucure such as GPS for he percepion componen of he sysem. Beyond he VT&R paradigm, here is a rich demonsraion of vision-based navigaion on UAVs in recen years [16]. While mos older demonsraions incorporae sereo sysems for scale, hey suffer from poor (i.e., small) baselineo-deph raios a higher aliudes. Recen advances in Inerial Measuremen Uni (IMU) echnology have allowed he use of loosely [17] [19] and ighly coupled [2] [22] visual-inerial sysems using boh monocular and sereo s [23], and wih impressive demonsraions of dynamic manoeuvres a high speed [24] in indoor, small-scale seups. The majoriy of large scale demonsraions using hese sysems, however, ofen exis as a full Simulaneous Localisaion and Mapping (SLAM) framework [25], [26], incorporaing exploraion and globally meric 3D maps as a mehod of accurae survey. In conras, VT&R akes a locally meric approach for map building, and leverages a human operaor for he iniial demonsraion ask, circumvening he difficul asks of auonomous exploraion and loop closures. III. METHODOLOGY In his paper, we use our well-esablished VT&R 2. sofware sysem as presened in [3], including he exension o a gimbal-sabilised [1]. However, we adap his sysem for use on a muliroor UAV specifically for he purposes of emergency reurn. Insead of each and repea phases, we implemen funcionally similar learn and reurn phases. In he learn phase, he UAV flies using auonomous GPS waypoin following or human operaor conrol. During his phase, he VT&R algorihm performs passive Visual Odomery (VO), insering he visual observaions from his privileged experience ino a relaive map of pose and scene srucure, effecively learning he roue. Following a primary navigaion sysems failure, he UAV should ener he reurn phase, and, wihou reliance on GPS or oher exernal sensing, auonomously re-follow he roue home in he reverse direcion. In addiion o performing he same VO as in learn, i performs a localisaion using a local segmen from he learn pah. The follows he learn pah by sending high-frequency localisaion updaes (relaive posiion and orienaion wih respec o he map) o a pah-following conroller. Once he reurns o he sar poin, i hovers unil aken over by a human conroller. To be clear, in his paper we only simulae GPS failures by manually commanding he o ener he reurn phase during fligh. In he following secions, we describe our VT&R sysem, including he archiecure of he sysem, he visual navigaion algorihm, and gimbal and pah-following conrollers. A. Sysem Overview The archiecure of he VT&R sysem for he muliroor UAV is shown in Fig. 2. All processing, including visual navigaion, localisaion, planning and conrol occurs on-board he UAV on he primary compuer (Fig. 1). This compuer direcly inerfaces wih he on-board via Universal Serial Bus (USB) 3., which provides greyscale sereo images for visual navigaion. This compuer also inerfaces wih he on-board auopilo via a serial Transisor-Transisor Logic (TTL) connecion, which provides daa (gimbal sae, auopilo sae, ec.) and he inerface for sending conrol commands. A long-range, low-bandwidh XBee 9 Mhz wireless link is used o communicae wih he primary onboard compuer from a ground saion. The ground saion compuer is uilised only for saus monioring and sending of high-level conrol commands. These commands consis of manual sae ransiion requess (swiching from learn o reurn), obaining fligh conrol auhoriy from he auopilo, and iniiaing GPS waypoin missions. The VT&R sofware sysem consiss of several ineracing componens (Fig. 2): 1) VO (including separae feaure exracion, pose esimaion and windowed refinemen hreads)

3 WARREN e al.: VISUAL TEACH & REPEAT FOR EMERGENCY RETURN OF MULTIROTOR UAVS DURING GPS FAILURE Fig. 2: The archiecure of he VT&R sysem for muliroor UAVs. 2) visual localisaion, 3) a sae machine, 4) a pah planner, 5) gimbal and pah-following conrollers and 6) a safey monior. Each sysem operaes in a separae hread or process, ineracing hrough he ransfer of daa caches (a packe of new and derived daa, including images, processed feaures and esimaed ransforms) and hrough he use of a Google Proobuf back-end for disk sorage. Memory managers ensure ha sale daa is wrien o disk o reduce Random Access Memory (RAM) uilisaion, and is pre-empively re-loaded during he reurn phase o ensure localisaion can proceed wihou waiing for disk access. The Robo Operaing Sysem (ROS) is used o run he safey monior and inerface o he auopilo and. The adaped VT&R sae machine for muliroor UAVs conrols he high-level sae ha he sysem is in (usually learn or reurn). A safey monior runs as an independen process o ensure safe operaion of he in case of sysem failure. I performs a saniy check on conrol and localisaion daa, in addiion o a wachdog funcionaliy on he conrol commands and sae daa boh from VT&R and he auopilo. Any moniored command or sae daa ha is delayed by more han a preconfigured imeou riggers a safey failure, forcing he o release auonomous conrol and rever o manual pilo conrol. In he following secions, he visual sysem, pah-following and gimbal conrollers are described in more deail. B. Visual Sysem The visual sysem consiss of separae hreads for feaure exracion, pose esimaion (VO), refinemen and localisaion, using images capured by a sereo o esimae boh pose updaes and localisaion o he pah during he reurn. 1) Visual Odomery: During boh he learn and reurn phases, image pairs are capured by a calibraed sereo a a frame rae of 15 Hz, while he gimbal sae (read as roll-, pich- and yaw-axis angular posiions) is capured a 1 Hz. The gimbal sae gives he pose of he in he frame by compounding he capured gimbal angles hrough a 3 series of ransforms wih known ranslaions exraced from a hand calibraion. We denoe he -o-sensor () ransform a ime τ as Tτsv. For each sereo image pair capured a ime, Speeded-Up Robus Feaures (SURF) feaures1 are exraced, descripors generaed and landmarks riangulaed. Landmarks are riangulaed from boh he sereo pair and from moion o accoun for boh close proximiy and exremely large dephs depending on aliude, similar o [22]. Each feaure in his laes framepair is mached o he las keyframe via SURF descripor maching on he Graphics Processing Uni (GPU). The raw maches are hen passed hrough a Maximum Likelihood Esimaion SAmple Consensus (MLESAC) robus esimaor o find he relaive ransform o he las keyframe. Finally, his ransform is opimised using our Simulaneous Trajecory Esimaion And Mapping (STEAM) bundle adjusmen engine [27], keeping landmarks fixed. Afer his process, if he number of inliers drops below a minimum coun or he moion (ranslaion or roaion) exceeds a hreshold, he frame is se as a keyframe and he feaures, new landmarks, and -o-sensor ransform a ha ime are sored in a verex in a pose graph for fuure rerieval. The relaive ransform is sored as an edge o he previous verex. Windowed bundle adjusmen (ermed windowed refinemen) is hen performed on he las 5-1 verices. This VO plus bundle adjusmen process generaes a dead-reckoned se of linked poses ha represen he pah. During he learn phase, his se of poses and edges is marked as privileged, as hey belong o he known, safe pah generaed from eiher human or GPS conrol. Naurally, incremenal ranslaional and roaional errors compound during his process, causing he global map o be disored. However, VT&R depends on he graph being only locally meric in he region o which he is localized. For a more horough explanaion of his componen, we direc he reader o our previous work [3]. 2) Visual Localisaion: During he reurn phase, while he flies in reverse, an addiional hread performs visual maching o he local map of 3D poins in he graph o esimae he pah-following error (Fig. 3), which is used by a pahfollowing conroller o keep he on he pah. To enable his process, he localisaion chain is used o keep rack of imporan verices in he graph and heir respecive ransforms. We use a ree model o name verices in he chain, going from he runk verex (defined as he closes verex spaially on he privileged pah), hrough he branch (he closes verex on he privileged pah wih a successfully MLESAC esimaed ransform o he curren pah), wig (he corresponding verex o he branch on he curren pah) and leaf (laes live verex) verices. These can be seen in Fig. 3. We use he noaion, b, w and l o refer o he runk, branch, wig and leaf verices, respecively. A every sep of VO (i.e., on every successfully esimaed frame, no jus keyframes) he localisaion chain is updaed wih he esimaed ransform from runk o leaf, (or T l = T f a = Tf e Teb Tba in Fig. 3). The leaf is updaed every sep 1 Alernaive feaure/descripor ypes such as ORB were esed, bu were demonsrably inferior in boh speed and reliabiliy o SURF for boh VO and localisaion in our experimenaion on his plaform.

4 4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED NOVEMBER, 218 Temporal edge Spaial edge (SE3) Privileged emporal edge Vehicle-sensor ransform (non-rigid) Fig. 3: During he reurn phase, he follows he learned roue in reverse. The localisaion chain updaes he esimaed localisaion ransform Ť fa a each VO updae. Upon creaion of a new verex F, visual localisaion insers he new edge T fa. The gimbal conroller minimises orienaion error of T fa, which includes -o-sensor ransform, T sv, and Ť fa. The uncerainies and some esimaed ransforms are omied here for clariy. and, if necessary, he runk verex is updaed o he closes esimaed privileged verex o he leaf. Upon inserion of a new VO keyframe as a verex in he graph, he localisaion hread aemps o esimae a new ransform from branch o wig. This process follows four separae sages: i) landmark migraion, ii) landmark maching, iii) pose esimaion, iv) opimisaion. Firs, he neares privileged verex (he runk) is used as he base verex o generae a local window of privileged verices ha conain poenially machable landmarks. Using he ransforms on he privileged edges, he landmarks in his window are ransformed o he runk o generae a locally meric se of 3D poins wih a common origin 2. Following a similar process o VO, feaures in he laes non-privileged verex (he leaf ) are mached using heir SURF descripors o all descripors of he migraed landmarks, which are hen passed hrough a MLESAC robus esimaor o esimae he relaive ransform from runk o leaf, T l (T fa in Fig. 3). Finally, he ransform is opimised while leaving all landmarks fixed. The localisaion chain is hen updaed o reflec his fresh ransform esimae, and he new branch o wig is se T wb T fa. The pah-following conroller can query he localisaion chain a any ime o ge he bes esimae of T l, faciliaing conrol a high speed even wih significan delays from visual localisaion. C. Gimbal Conroller Use of a gimbal decouples he visual perspecive from he orienaion changes of muliroor UAVs due o heir underacuaed conrol behaviour. This significanly improves he robusness of VT&R in he air by adding exra degrees of acuaion o he visual servoing problem. During fas, dynamic 2 While our previous work incorporaes feaures and poins from muliple experiences (i.e., muliple raverses), he learn-reurn framework by definiion only uses a single experience: he privileged one. manoeuvres, a gimbal-sabilised sysem will be able o ouperform a saic sysem by decoupling he aircraf moion from he view. In addiion, mainaining a consisen roll ensures ha generally unsable poin feaures are racked more consisenly. The gimbal is hand-calibraed o esimae he ranslaions beween each acuaed axis, and his proves sufficien for mos applicaions. This calibraion is used in conjuncion wih axis encoder values provided by he auopilo o esimae he sensor-o- ransform T sv, i.e. from he frame o he frame hrough he gimbal. Gimbal encoder daa is provided a 1 Hz wih an unspecified laency. In pracice, he laency is less han 1 ms. During he learn phase, gimbal conrol is no performed by VT&R, bu lef open-loop such ha he gimbal inernal conroller performs sabilisaion of roll and pich, and smoohes yaw ha follows he yaw. The value of his sensoro- ransform T τ sv (Fig. 3) is recorded a each new verex, corresponding o ime τ. During he reurn, he gimbal is acively conrolled by VT&R for he pich and yaw axes. The gimbal is commanded o reduce orienaion error beween he curren (leaf ) view and neares privileged (runk) view, ( T fa in Fig. 3), as knowledge of he ransform beween he curren and he privileged poses is known via he localisaion chain such ha: T fa = T l svt l T sv 1 using he sensor-o- ransforms capured a verices and l. Ť fa is updaed in he localisaion chain a every frame. D. Pah-Following conroller A pah-following conroller is implemened for conrol during he reurn phase o keep he as close as possible o he oubound pah while mainaining a suiable arge velociy. To enhance robusness o environmenal disurbances and sysem delays, we consider a pah-following approach, which, in conras o rajecory racking, prioriizes spaial error over emporal error [28]. By exending he approach in [28] o a VT&R framework, we achieve simple muliroor pahfollowing. This is done by convering a sandard P-D racking conrol o selec he spaially closes reference poin on he pah a each conrol ime sep (5 Hz). The localizaion chain provides an esimaed ransform from runk o leaf Ť l from which we can exrac he curren posiion p l = (x, y, z) and orienaion C l using: Ť l = [ ] Cl p l T = Ť 1 1 l. We obain a ranslaional velociy esimae v l = (ẋ, ẏ, ż) using STEAM rajecory generaion [27], which fis a consan velociy rajecory hrough he previous pah verexes. a) VT&R Pah-Following Reference: We generae a pah by connecing a sraigh-line hrough successive privileged verices. To do his, we use he localizaion chain o obain a ransform from he nex privileged verex o he runk T n. (1)

5 WARREN e al.: VISUAL TEACH & REPEAT FOR EMERGENCY RETURN OF MULTIROTOR UAVS DURING GPS FAILURE 5 From his we can exrac he posiion p n verex wih respec o he runk using T n = [ Cn p n T 1 ]. of he nex privileged A each ime sep, we deermine he reference posiion p ref = (x ref, y ref, z ref ) by projecing our curren muli-roor posiion p l ono he sraigh-line segmen connecing he runk o he nex privileged verex using: p ref = p l p n p n p n. We obain a reference velociy v ref = (ẋ ref, ẏ ref, ż ref ), where he magniude is a user-seleced parameer v des, in he direcion of he nex privileged verex using: p n v ref = v des p n. b) Conrol Design: Our pah-following conrol is designed o send commands (ż cmd, ψ cmd, θ cmd, φ cmd ) where ż cmd is a commanded z-velociy, ψ cmd is a command yaw rae, and θ cmd and φ cmd are commanded pich and roll, respecively. The z-velociy command is designed using a P-D conroller: ż cmd = 2ζ z (z ref z) + 1 τ z τz 2 (ż ref ż), (2) where ζ z and τ z are uned damping raio and ime consan. The curren yaw, ψ, wih respec o he runk is deermined from he roaion marix, C l. As seen in (3), a P-conroller (wih uned ime consan τ ψ ) is used o correc for any yawmismach beween he leaf and he runk: ψ cmd = 1 τ ψ ψ. (3) As in [29], laeral-moion conrol commands are deermined by firs designing ranslaional acceleraion commands using P-D conrol: a x = 2ζ θ (x ref x) + 1 τ θ τθ 2 (ẋ ref ẋ), (4a) a y = 2ζ θ (y ref y) + 1 τ θ τθ 2 (ẏ ref ẏ), (4b) where ζ θ and τ θ are uned damping raio and ime consan. Assuming small laeral acceleraion (ẍ ÿ ) and using sandard feedback linearizaion, hese linear acceleraion commands are ransformed ino pich and roll commands: θ cmd = arcsin( a x g cos ψ + a y sin ψ), g (5a) φ cmd = arcsin( a x g sin ψ + a y cos ψ), g (5b) where g is he graviaional consan. In general, his conroller can follow any arbirary pah, as long as verices are placed sufficienly close ogeher o accuraely reflec he rue pah. However, pah-curvaure and large direcion changes are no accouned for by his conroller, he resuls of which will be examined in Secion IV. Fig. 4: Overview of he rajecory flown a he DRDC Suffield Research Cenre, shown in magena. Velociy profile for a arge 15 m/s commanded speed overlaid. IV. EXPERIMENTS To evaluae he performance of he airborne VT&R algorihm, a number of oudoor experimens were performed onboard he arge UAV. In he firs experimen, we evaluae he performance of he localisaion algorihm under GPS conrol using he described gimbal conroller. Specifically, we es he performance of he localisaion algorihm and gimbal conroller under deliberaely challenging condiions, including high-speed, dynamic fligh and high learn vs. reurn posiional error. For his experimen, we deliberaely exclude he conroller o isolae he performance of he subcomponens of he algorihm. In he second experimen, we perform closed-loop conrol wih he aforemenioned pah-following conroller developed for full 6- DOF moion. This sysem is evaluaed over several runs, showing he full sysem operaing. The experimenal seup is described in he following subsecion. A. Experimenal Seup For hese experimens, we use a DJI Marice 6 Pro, wih aached Ronin-MX gimbal (Fig. 1). This sysem has a akeoff weigh of approximaely 1 kg, and maximum span roorip-o-ip of 1.64 m. Conrol is provided by a DJI A3 riple redundan auopilo. On-board his sysem is an NVIDIA Tegra TX2 module (6 ARM cores core Pascal GPU) and SereoLabs ZED sereo conneced via USB3, boh mouned in he sabilised plaform of he Ronin-MX gimbal. The Marice 6 Pro provides sae informaion o VT&R running on-board he TX2, including gimbal encoder posiions and GPS saus, while he ZED provides greyscale imagery wih resoluion a 15 Hz. The Tegra TX2 runs NVIDIA L4T v28.2, a varian of Ubunu 16.4 for ARM archiecures. The primary locaion used for he experimens in his paper is a simulaed village a he Defence Research & Developmen Canada (DRDC) Suffield Research Cenre in souhern Albera, Canada. The Suffield locaion consiss of a number of shipping conainers placed o emulae buildings and narrow alleys in fla grassland, suied o a simulaed parol scenario.

6 6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED NOVEMBER, 218 Number of localisaion inliers m/s 7m/s 8m/s 1m/s 12m/s 15m/s Number of localisaion inliers m 14m 16m 18m 3m/s 7m/s 8m/s 1m/s 12m/s 15m/s Targe velociy Fig. 5: Localisaion performance is comparable wih increasing arge (and average) velociy, where learn and reurn phases are conduced a he same speed. B. Localizaion Performance Evaluaion In hese experimens, we evaluae he combined performance of he localisaion algorihm and gimbal conroller o successfully localise he under increasingly difficul operaional condiions. We es his in wo ways: increasing arge velociy of he, and deliberaely offse aliudes on he oubound and reurn pahs. The firs es shows he performance under increasingly dynamic manoeuvres of he, inducing rapid perspecive change and poor pah racking, which mus be aenuaed by he gimbal conroller. The second es shows he performance of he localisaion algorihm wih inenionally poor perspecive. We deliberaely do no use he conroller in hese ess o decouple and isolae he performance of he localisaion algorihm and gimbal conroller. 1) Increasing Targe Velociy: For his experimen, he aircraf is auonomously flown a 12 m Above Ground Level (AGL) along he pah depiced in Fig. 4 in a clockwise direcion. VT&R is placed ino learn mode, before he oubound roue is flown under auonomous conrol, by uploading a waypoin mission o he Marice 6 auopilo. Once he reaches he end of he loop, VT&R is swiched o reurn mode, and he aircraf is again auonomously commanded o reurn along he same pah by following he waypoins in reverse. During his reurn sage, he gimbal is acively conrolled by VT&R o reduce orienaion errors caused by pah-following discrepancies generaed by he GPS-based conroller. The roue is flown a increasingly fas arge speeds, 3, 7, 8, 1, 12 and 15 m/s, on boh he learn and reurn sages. While he reaches his speed during only pars of his pah, he average speed also increases wih each pass. A ypical speed profile for he pah a 15 m/s arge speed is shown in Fig. 4. Fig. 5 shows he median and variance of he localisaion inliers recorded along he reurn pah for each of he arge speeds. This figure shows ha even a he highes commanded speed (15 m/s), localizaion performance remains similar o hose examples a lower speeds. A 15 m/s some localisaion failures occur, bu he majoriy of hese can be aribued o failures of he hardware gimbal conroller during a segmen of he pah. 2) Increasing Heigh Error: For his experimen, he aircraf is again auonomously flown a 12 m AGL during he 12m 14m 16m 18m Targe aliude Fig. 6: Successful localisaion occurs wih significanly increasing aliude difference beween learn (12 m) and reurn phases (esed a 12, 14, 16 and 18 m), bu he average (green) shows decline a more exreme (5%) differences. Orienaion error ( ± ) m/s 7m/s 8m/s 1m/s 12m/s 15m/s Experimen Fig. 7: While aiude error increases in boh median and variance wih increasing arge velociy, he gimbal conroller mainains a consisen orienaion beween learn and reurn regardless of arge speed. learn sage along he pah depiced in Fig. 4 in a clockwise direcion a a nominal speed of 7 m/s. The oal lengh of he pah is approx. 45 m. Once he reaches he end of he loop, VT&R is swiched o reurn mode, and he aircraf is again auonomously commanded o reurn along he same pah by following he waypoins in reverse a he same 7 m/s arge speed. In his case, however, we vary he aliude a which he aircraf reurns, o es he robusness of he gimbal conroller and abiliy of he algorihm wih large posiional offses. In hese experimens, we show he localisaion inliers along he pah wih arge reurn heighs of 12, 14, 16 and 18 m, respecively (Fig. 6). In Fig. 6, localisaion performance is sill high, wih an average of 1 inliers per keyframe, even a aliude differences of 6 m, or 5%. While some of his performance can be aribued o perspecive due o he aliude, a significan componen can be aribued o he gimbal compensaing for he reduced image overlap ha would be presen on a saic. Imporanly, however, he average inliers does drop significanly, and more ineresingly, reduces in variance. This is likely due o he enhanced viewpoin overlap (of he learn pah) a higher aliudes, meaning posiional errors have less effec on mainaining observabiliy of all landmarks during localisaion. Finally, Fig. 7 shows he uiliy of he gimbal in minimising perspecive error caused by differing aiudes beween

7 WARREN e al.: VISUAL TEACH & REPEAT FOR EMERGENCY RETURN OF MULTIROTOR UAVS DURING GPS FAILURE 7 Feaure Exracion Visual Odomery Windowed Refinemen feaures»15hz keyframes»15hz Feaure Exracion Sereo Triangulaion Landmark Recall»»2-4Hz Feaure Maching MLESAC Bundle Adjusmen Triangulaion Submap Esimaion Landmark Migraion Localisaion keyframes»»2-4hz Execuion Time (ms) Fig. 8: Average execuion imes for he visual pipelines on he TX2 during a mission, separaed by hread of execuion. Once one hread is finished processing, i is able o process he nex image and is daa producs. learn and reurn. Due o he obvious underacuaed naure of muliroor sysems, acceleraions and deceleraions cause he aiude o differ beween hese wo passes of he pah. For a saic sysem, hese differences can cause performance degradaion due o poor image overlap. Using a gimbal wih acive conrol o aenuae orienaion error can minimise his effec. Fig. 7 shows he magniude of orienaion error for wo separae localisaion ransforms a each speed profile (read as a pair) aken from he esimaed localisaion chain as esimaed from he visual pipeline: in he frame (T l, or T fa in Fig. 3) on he lef, and in he frame ( T fa in Fig. 3) on he righ. As can be seen a all speed profiles, he gimbal succeeds in minimising he orienaion of he localisaion ransform, and his performance is relaively consisen wih increasing speed. In his scenario, he arge speeds of he learn and reurn phases are he same for each speed profile, meaning here will be some consisency in orienaion in boh phases. Wih differing speed profiles, we would expec he observed uiliy of he gimbal-sabilised o increase furher. 3) Execuion Time: Fig. 8 shows he average execuion ime for he separae processes in he VT&R algorihm onboard he Tegra TX2. While feaure exracion and VO process every image pair a an approximae speed of 66 ms ( 15 Hz), windowed refinemen only runs on generaion of a keyframe, and localisaion runs afer his process is complee. Feaure exracion and maching are all performed on he GPU. Using his hreaded seup allows VT&R o run online. C. Full VT&R Evaluaion In his experimen, we evaluae he performance of he full closed-loop VT&R sysem, using GPS navigaion during he learn phase, and swiching o he presened pah-following conroller for he reurn phase. Over hree separae rials, each consising of a single fligh, we raverse he pah shown in Fig. 4 in a clockwise direcion a an aliude of 12 m AGL, before reurning in an aniclockwise direcion a he same aliude (aemping o minimise all posiional errors) a a arge speed of 3 m/s. In all hree rials, VT&R was able o complee he reurn phase of fligh under pah-following conrol over an approximaely 2.5 minue period. Fig. 9 shows he pah for hese rials. The oubound pah under GPS conrol is shown in dashed blue, while he reurn pah under pah-following conrol is shown in ligh blue, green and red for rials T1, T2 and T3, respecively. Figs. 1 and 11 show he normalised cross-rack error (in Y y (m) Æ4 T1 T2 T3 GPS Æ2 Æ3 Æ x (m) Fig. 9: The oubound (GPS, dashed blue) and reurn (conroller, ligh blue, green, red) pahs during for hree separae rials. Some offse is seen on sharp urns. Velociy profile for rial T3 overlaid. Normalised Y-Z disance o privileged pah (m) Æ4 T1 T2 T3 GPS ø reurn direcion Disance from origin (m) Fig. 1: Pah-following error is of a similar order o ha for GPSbased conrol over he majoriy of he pah using our conroller, according o he localisaion ransform esimaed by VT&R. # of inlier maches Æ4 ø reurn direcion Æ3 Æ2 Æ3 Æ2 Æ1 Æ Disance from origin (m) T1 T2 T3 GPS Fig. 11: The number of localizaion inliers while using our pahfollowing conroller is of a similar order o ha for GPS-based conrol over he majoriy of he pah. Velociy (m/s)

8 8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED NOVEMBER, 218 and Z, using he vision-based esimae) and number of inlier maches respecively for each rial in comparison o a reurn fligh under GPS conrol. Specific segmens of he pah are highlighed in he inse figures of Fig. 9 and annoaed wih numbers ha correlae o hose in Figs The posiional error is less han 1.5 m over mos of he pah using he pah-following conroller (Fig. 1), and is comparable o a reurn rajecory under GPS conrol, showing he srong performance of a simple vision-based pah-following conroller compared o his primary sensor. In specific secions such as corners, however, cross-rack error increases o a maximum of 4.5 m. This can be aribued o he simpliciy of he conroller, as curvaure of he pah is no accouned for, and velociy error is weighed higher han cross-rack error. Despie hese pah-following errors, localisaion performance is srong over he full rajecory (Fig. 11), wih no localisaion failures, even a he highlighed corner poins. The average performance over he rajecory is again comparable o a reurn phase under GPS conrol. V. CONCLUSIONS In his paper, a full VT&R sysem for emergency reurn of a muliroor UAV has been presened. Using imagery from a gimbal-sabilised sereo o build a map online during a commanded learn phase, auonomous reurn of he in he same fligh has been demonsraed by maching live landmarks o he map for auonomous pah-following conrol. In addiion, we have demonsraed he robusness of he gimbal-sabilised sysem o high-speeds and large posiional errors. Fuure work will focus on a more advanced pah-racking conroller o minimise cross-rack errors, and esing in a muli-experience framework over a long-erm experimen. REFERENCES [1] P. Furgale and T. D. Barfoo, Visual Teach and Repea for Long-Range Rover Auonomy, Journal of Field Roboics, vol. 27, no. 5, pp , 21. [2] C. J. Osafew, A. P. Schoellig, and T. D. Barfoo, Visual Teach and Repea, Repea, Repea: Ieraive Learning Conrol o Improve Mobile Robo Pah Tracking in Challenging Oudoor Environmens, in Inernaional Conference on Inelligen Robos and Sysems, IEEE, 213. [3] M. Paon, K. Macavish, M. Warren, and T. D. Barfoo, Bridging he Appearance Gap : Muli-Experience Localizaion for Long-Term Visual Teach and Repea, in Inernaional Conference on Inelligen Robos and Sysems, 216. [4] T. Krajník, J. Faigl, V. Vonásek, K. Košnar, M. Kulich, and L. Peučil, Simple Ye Sable Bearing-Only Navigaion, Journal of Field Roboics, vol. 27, no. 5, pp , 21. [5] M. Paon, F. Pomerleau, and T. D. Barfoo, Eyes in he Back of Your Head: Robus Visual Teach & Repea Using Muliple Sereo Cameras, in Canadian Conference on Compuer and Robo Vision, 215. [6] A. Pfrunder, A. P. Schoellig, and T. D. Barfoo, A Proof-of-Concep Demonsraion of Visual Teach and Repea on a Quadrocoper Using an Aliude Sensor and a Monocular Camera, in Canadian Conference on Compuer and Robo Vision, 214. [7] M. Warren, M. Paon, K. MacTavish, A. P. Schoellig, and T. D. Barfoo, Towards Visual Teach & Repea for GPS-Denied Fligh of a Fixed-Wing UAV, in Field and Service Roboics: Resuls of he 11h Inernaional Conference, pp , 217. [8] A. G. Toudeshki, F. Shamshirdar, and R. Vaughan, UAV Visual Teach and Repea Using Only Semanic Objec Feaures, in Canadian Conference on Compuer and Robo Vision, (Torono), 218. [9] J. Surber, L. Teixeira, and M. Chli, Robus Visual-Inerial Localizaion wih Weak GPS Priors for Repeiive UAV Flighs, in Inernaional Conference on Roboics and Auomaion, 217. [1] M. Warren, A. P. Schoellig, and T. D. Barfoo, Level-Headed: Evaluaing Gimbal-Sabilised Visual Teach and Repea for Improved Localisaion Performance, in Inernaional Conference on Roboics and Auomaion, (Brisbane), 218. [11] N. Playle, Improving he Performance of Monocular Visual Simulaneous Localisaion and Mapping hrough he use of a Gimballed Camera. Masers, Universiy of Torono, 29. [12] C. S. Sharp, O. Shakernia, and S. Sasry, A Vision Sysem for Landing an Unmanned Aerial Vehicle, in Inernaional Conference on Roboics and Auomaion, 21. [13] A. Borowczyk, D.-. Nguyen, A. P.-V. Nguyen, D. Q. Nguyen, D. Saussie, and J. L. Ny, Auonomous Landing of a Quadcoper on a High-Speed Ground Vehicle, Journal of Guidance, Conrol, and Dynamics, vol. 4, no. 9, pp , 217. [14] C. E. Lin, Camera Gimbal Tracking from UAV Fligh Conrol, in CACS Inernaional Auomaic Conrol Conference, pp , 214. [15] C. L. Choi, J. Rebello, L. Koppel, P. Gani, A. Das, and S. L. Waslander, Encoderless Gimbal Calibraion of Dynamic Muli-Camera Clusers, in Inernaional Conference on Roboics and Auomaion, (Brisbane), 218. [16] A. Giusi, J. Guzzi, D. C. Cire, F.-l. He, J. P. Rodríguez, F. Fonana, M. Fässler, C. Forser, J. Schmidhuber, G. D. Caro, D. Scaramuzza, and L. M. Gambardella, A Machine Learning Approach o Visual Percepion of Fores Trails for Mobile Robos, IEEE Roboics and Auomaion Leers, vol. 1, no. 2, pp , 216. [17] M. W. Achelik, M. C. Achelik, S. M. Weiss, and R. Siegwar, Onboard IMU and Monocular Vision Based Conrol for MAVs in Unknown Inand Oudoor Environmens, in Inernaional Conference on Roboics and Auomaion, pp , IEEE, may 211. [18] S. M. Weiss, D. Scaramuzza, and R. Siegwar, Monocular SLAM Based Navigaion for Auonomous Micro Helicopers in GPS Denied Environmens, Journal of Field Roboics, vol. 28, no. 6, pp , 211. [19] S. M. Weiss, M. W. Achelik, S. Lynen, M. C. Achelik, L. Kneip, M. Chli, and R. Siegwar, Monocular Vision for Longerm Micro Aerial Vehicle Sae Esimaion: A Compendium, Journal of Field Roboics, vol. 3, no. 5, pp , 213. [2] T. Hinzmann, T. Schneider, M. Dymczyk, A. Schaffner, S. Lynen, R. Siegwar, and I. Gilischenski, Monocular Visual-Inerial SLAM for Fixed-Wing UAVs Using Sliding Window Based Nonlinear Opimizaion, in Inernaional Symposium on Visual Compuing, pp , Springer Inernaional Publishing, 216. [21] C. Forser, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, SVO: Semidirec Visual Odomery for Monocular and Muli Sysems, IEEE Transacions on Roboics, vol. 33, no. 2, pp , 217. [22] R. Mur-Aral and J. D. Tardos, ORB-SLAM2: An Open-Source SLAM Sysem for Monocular, Sereo, and RGB-D Cameras, IEEE Transacions on Roboics, vol. 33, no. 5, pp , 217. [23] K. Sun, K. Moha, B. Pfrommer, M. Waerson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, Robus Sereo Visual Inerial Odomery for Fas Auonomous Fligh, IEEE Roboics and Auomaion Leers, vol. 3, no. 2, pp , 218. [24] G. Loianno, C. Brunner, G. McGrah, and V. Kumar, Esimaion, Conrol, and Planning for Aggressive Fligh Wih a Small Quadroor Wih a Single Camera and IMU, IEEE Roboics and Auomaion Leers, vol. 2, no. 2, pp , 217. [25] S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar, Muli-Sensor Fusion for Robus Auonomous Fligh in Indoor and Oudoor Environmens wih a Roorcraf MAV, in Inernaional Conference on Roboics and Auomaion, 214. [26] T. Qin, P. Li, and S. Shen, Relocalizaion, Global Opimizaion and Map Merging for Monocular Visual-Inerial SLAM, in Inernaional Conference on Roboics and Auomaion, (Brisbane), 218. [27] S. Anderson and T. D. Barfoo, Full STEAM ahead: Exacly Sparse Gaussian Process Regression for Bach Coninuous-Time Trajecory Esimaion on SE(3), in Inernaional Conference on Inelligen Robos and Sysems, sep 215. [28] J. E. Hauser and R. Hindman, Maneuver Regulaion from Trajecory Tracking: Feedback Linearizable Sysems, 3rd IFAC Symposium on Nonlinear Conrol Sysems Design, vol. 28, no. 14, pp , [29] S. Spedicao, A. Franchi, and G. Noarsefano, From Tracking o Robus Maneuver Regulaion : an Easy-o-Design Approach for VTOL Aerial Robos, in Inernaional Conference on Roboics and Auomaion, 216.