Autonomous Exploration in Unknown Urban Environments for Unmanned Aerial Vehicles

Transcription

1 Autonomous Exploraton n Unknown Urban Envronments for Unmanned Aeral Vehcles Davd Hyunchul hm * and Hoam Chung Unversty of Calforna, Berkeley, CA, 9470 H. Jn Km eoul Natonal Unversty, eoul, Korea and hankar astry Unversty of Calforna, Berkeley, CA, 9470 In ths paper, we present an autonomous exploraton method for unmanned aeral vehcles n unknown urban envronment. We address two major aspects of exploratongatherng nformaton about the surroundngs and avodng obstacles n the flght path- by buldng local obstacle maps and solvng for conflct-free trajectory usng model predctve control (MPC) framework. For obstacle sensng, an onboard laser scanner s used to detect nearby objects around the vehcle. An MPC algorthm wth a cost functon that penalzes the proxmty to the nearest obstacle replans the flght path n real-tme. The adjusted trajectory s sent to the poston trackng layer n the UAV avoncs. The proposed approach s mplemented on Berkeley rotorcraft UAVs and successfully tested n a seres of flghts n urban obstacle setup. I. Introducton NMANNED aeral vehcles (UAVs) have become an ndspensable ngredent for many applcatons where U human operaton s consdered unnecessary or too dangerous. As UAVs fnd ther way nto more demandng applcatons such as low-alttude ground support or urban operatons, they are expected to fly autonomously wthout colldng nto obstacles. The capablty to avod obstacles has not been of major concerns to conventonal UAVs, whch are typcally operated at hgher alttudes free from any obstacles and closely montored by human operators. However, as UAVs are requred to operate n cluttered or dynamcally changng envronments wthout external support, the autonomous exploraton capablty has become as a crucal component technology for future UAVs. Explorng autonomously n an unknown envronment requres map buldng and onlne trajectory replannng for collson-free navgaton. These topcs have been ntensvely covered by robotcs socety snce late 70s. As a result of these efforts, varous algorthms and mplementatons are currently avalable for gudng moble robots n unknown or partally known two-dmensonal world. Although some of those algorthms can be extended to threedmensonal problems, these are stll computatonally expensve, and have lmtatons n three-dmensonal outdoor applcatons. In the 1990s, researchers ntroduced probablty theores nto map buldng technques, 1 enhancng robustness and performance of those algorthms even wth less costly but naccurate sensors. Not surprsngly, ntensve computatons are requred for these enhancements, and the stuaton becomes worse n three-dmensonal problems. UAVs also pose more challenges than the ground robots do n the development, mplementaton, and operaton: they typcally fly at a much faster speed than the ground robots, and therefore requres faster and more accurate decson makng. The payload allocated to the avoncs and onboard sensors s typcally more lmted than * Prncpal Development Engneer, Department of Electrcal Engneerng and Computer cence, Correspondng Author (hcshm@eecs.berkeley.edu) Ph.D. Canddate, Department of Mechancal Engneerng (hachung@eecs.berkeley.edu). Assstant Professor, chool of Mechancal and Aerospace Engneerng (hjnkm@snu.ac.kr). Professor, Department of Electrcal Engneerng and Computer cence (sastry@eecs.berkeley.edu). 1 Amercan Insttute of Aeronautcs and Astronautcs

2 ground robots. Fnally, durng the development stage, typcal tral-and-error approaches are not usually possble because falures to avod obstacles nescapably lead to costly and very dangerous accdents. Model predctve control has been found attractve for addressng control problems n dynamc envronments. The onlne optmzaton 3 wth fnte horzon enables a control system more responsve to the changes n the system dynamcs and the surroundngs. Further, t has been explored and proven effectve that a varety of performance goals, n addton to the feedback stablzaton, can be ncorporated nto the cost functon: hm, Km, and astry 4 proposed MPC-based flght control algorthms by ntroducng a set of cost functons for decentralzed collson avodance and aeral pursut-evason. 5 Partcularly, t s shown that MPC s capable of obstacle avodance usng a cost functon that penalzes the proxmty to the nearest obstacle. The nformaton about obstacles n the flght path can be made avalable to the path planner by a preprogrammed map or a dynamcally bult map. Whereas the former approach does not suffer from any sensornduced errors, the map tself can be naccurate or outdated. Therefore, we favor dynamc map buldng usng onboard sensors. aser range fnders, whch measure the dstance from the sensor to objects around, has been found very attractve due to ts accuracy and long detecton range. In ths paper, we propose an autonomous exploraton algorthm sutable for, but not lmted to, urban navgaton by combnng the MPC-based obstacle avodance wth local obstacle map buldng usng onboard laser scannng. tartng from the gven trajectory, the MPC layer solves for a collson-free trajectory by the real-tme gradentsearch based optmzaton. The proposed framework s valdated n smulatons, and then tested n a seres of experments usng a smulated urban envronment as shown n Fgure 1. II. Formulaton In ths secton, we provde some background n the UAV system dynamcs, MPC formulaton, and the coordnate transformaton for laser scannng. A. ystem Dynamcs and Path Generaton usng MPC A model for a gven system dynamcs can be wrtten as a dscrete-tme dynamc equaton such that: x( k + 1) = f ( x( k)) + g( x( k)) u ( k) (1) We are nterested n solvng a dscrete-tme optmal control problem for the system n Eq. (1) to fnd the optmal * nput sequence { k } u ( ) T such that k = 1 subject to the dynamc equaton (1). * T T { ( k) } = arg mn q ( ( k), ( k) ) + q f ( ( T + 1) ) u x u x () k = 1 k = 1 * Nonlnear model predctve control (NMPC) problem solves for the optmal control law { u ( k) } T k = 1 * x and mplement the optmal nput { u } nonlnear dynamc equaton n Eq. (1), startng from (1) Fgure 1. Berkeley UAV flyng autonomously n a smulated urban envronment under the ( ) k k τ = 1 1 τ T and then repeat these steps from the state x( τ + 1) at k = τ + 1. The path plannng strategy used n ths paper combnes, as orgnally proposed n Ref. 4, the potental feld concept wth the NMPC-based onlne optmzaton. The cost functon n Eq. () ncludes potental functon terms responsble for path plannng n the envronment wth statonary or movng obstacles. Ths allows the trajectory for Amercan Insttute of Aeronautcs and Astronautcs

3 generaton and vehcle stablzaton to be combned nto a sngle optmzaton problem, and the look-ahead nature of the MPC framework makes ths approach less vulnerable to local mnma. 4 For obstacle avodance capablty, the followng potental functon term s ncluded n the cost functon q x( k), u ( k) : ( ) q obst K ( x ( k)) =, (3) a x k x k + b y k y k + c z k z k + ε N = 1 ( ( ) ( )) ( ( ) ( )) ( ( ) ( )) where ( x, y, z ) denotes the poston of the vehcle, and ( x, y, z ) denotes the poston of the -th nearest obstacle (or the poston of other vehcles) n local Cartesan coordnate frame. Eq. (3) ntroduces a potental feld near N obstacle ponts nto the MPC framework. ( a, b, c ) s a set of scalng factors n x, y, z drectons, respectvely. s a postve constant to prevent Eq. (3) from beng sngular when ( x, y, z ) = ( x, y, z ). In ecton III, we wll present an obstacle avodance algorthm usng the MPC framework shown above for urban exploraton problems. B. Coordnate Transformaton for aser can Data The laser scanner used n ths research conssts of a laser source, a laser receptor and a rotatng mrror for planar scannng. An accurate tmng devce measures the tme lapse between the moment the laser beam s emtted from the source and the moment the reflected laser beam returns to the receptor. The rotatng mrror reflects the laser beam n a plane, enablng two-dmensonal scannng. At each scan, the sensor reports a stream of measurements that supples the followng measurement set: {(, β ), 1,..., } Y = d n = N, (4) n n meas where d n, β n, and N meas represent the dstance from an obstacle, the angle n the scannng plane, and the total number of measurements per scan, respectvely. Each measurement can be wrtten nto a vector form such that where and j are orthonormal unt vectors lyng n the scannng plane. The subscrpts D and represent scanned data and laser scanner, respectvely. The calculaton of the spatal coordnates of detected pont nvolves a seres of coordnate transformatons among three coordnate systems: body coordnate systems attached to the laser scanner and to the host vehcle, respectvely, and the spatal coordnate system, to whch the vehcle locaton and atttude are referred. To ensure conflct-free navgaton n an arspace flled wth obstacles, the laser scanner should be able to scan the area wde so that the entre vehcle can pass through wth safety margns. For example, f the laser scanner s nstalled to scan the area ( cos β sn β ) =, (5) D / n dn n + nj horzontally, an actuaton n the ptch axs s necessary so that the scanner can cover the area suffcently hgher than the rotor plane and lower than the landng gear. Each laser measurement vector n laser scanner-attached body coordnates s frst transformed nto the vehcleattached body coordnates and then the spatal coordnate system as followng: Z B B Z scanner obstacle Fgure. Coordnate transformatons for laser scan data B V / B Z D / = R D / / D / = R R ( α) / B B / D /, (6) 3 Amercan Insttute of Aeronautcs and Astronautcs

4 where the subscrpts and B represent spatal- and body-coordnate system, respectvely. R / ( ) s the transformaton matrx from the laser body coordnate to vehcle body coordnate B where s the tlt angle wth respect to the vehcle body. R / B denotes the transformaton matrx from vehcle body coordnates to spatal coordnates. Fnally, the spatal coordnate of the obstacle s found by: B α = + + D D / / B B = R R ( α) + R + B / B B / D / / B / B B (7) Usng Eq. (7), one can fnd the spatal coordnate of a detected obstacle pont by combnng the raw measurement vector wth the poston and atttude of the vehcle, whch s avalable from the IN and the GP onboard. In Eq. (7), the accuracy of the detecton n the spatal coordnate system not only depends on the laser scanner tself, but also the accuracy of the vehcle poston and atttude. Fgure 3 shows fxed and actuated types of laser scanner mountngs on Berkeley UAVs. The scanner shown n the left s mounted on a fxture rgdly attached to the helcopter body whereas one on the rght s nstalled on a tlt actuator wth a tlt angle encoder. The laser scanner on a fxed mountng turns out to provde scannng over a lmted vertcal range due to the small body moton. Fgure 3. aser scannng devces mounted on Berkeley UAVs (left: fxed, rght: actuated mountng) III. Autonomous Exploraton usng MPC wth ocal Maps In ths secton, we present an MPC-based trajectory generaton for autonomous exploraton n an unknown envronment wth obstacles. Partcularly, we are nterested n addressng safe navgaton of UAV n urban envronments wth no pror nformaton avalable on the obstacles. We begn wth the followng statement: Problem tatement Fnd a trajectory that allows the vehcle to navgate from the gven startng pont A to the destnaton pont B wth safe dstance from obstacles n the envronment. We address the problem wth an ntegral approach of MPC-based trajectory planner wth local obstacle map generaton usng onboard sensors. A. Trajectory Replannng wth MPC In ths secton, we consder a navgaton problem from pont A to pont B, connected by a reference trajectory. Wthout loss of generalty, the trajectory s assumed as a straght lne. The MPC approach n ecton II-A can be formulated as a trackng control problem wth a cost functon term for Eq. () such that 4 Amercan Insttute of Aeronautcs and Astronautcs

5 where ( ( )) trk obst q x k = q ( x( k)) + q ( x ( k)) n Eq. (). trk 1 T q ( x( k)) = ( y ref ( k) x( k) ) Q( y ref ( k) x ( k) ), (8) In Ref. 4, Eq. (1) s chosen to be the full vehcle dynamc model so that the optmzed varable u ( k) s the stablzng control nput that also mnmzes other penaltes for trackng, obstacle avodance, or aeral pursutevason games. In ths research, the MPC optmzaton s appled to a smplfed vehcle dynamcs and we solve for the nput, whch s then converted to the reference trajectory. The advantage of dong so s, by separatng the trajectory layer from the stablzaton layer, any falure of the optmzaton routne to converge to a soluton can be decoupled wth the overall stablty of the vehcle. However, the dfference between the reference poston and the physcal poston due to the trackng error should be consdered n the obstacle map buldng process n ecton III-B, whch becomes somewhat complcated. The system dynamcs s chosen to be as: T x( k + 1) = x( k) + T u ( k), (9) T where x [ x y z ], u vx vy v z and T s the samplng tme of the dscretzed model. In ths setup, s the optmzaton results n the reference velocty n the spatal coordnates. The reference trajectory s obtaned by solvng the forward dfference equaton (9) wth the reference velocty. The trackng layer n Berkeley UAV avoncs 6 s responsble to follow t wth mnmal error. In Ref. 4, t was shown that the cost term (Eq. (3)) wth N=1 s suffcent although Eq. (3) wth N > 1 s expected to result n a smoother cost functon surface and thus allow a better convergence n the gradent-search based optmzaton. In favor of effcency of algorthms and computaton, we choose the nearest-pont method,.e., N=1 so that only the nearest pont s consdered n the optmzaton(fgure 4). It s noted, however, the nearest pont s not dentcal throughout the fnte horzon of each step as the state varables ncludng the vehcle poston propagates over the hypothetc tme. The cost term n Eq. (3) for urban navgaton s therefore set to s ( mn mn mn ) obst 1 q ( x ( k)) = K ( x ( k) x ( k)) + ( y ( k) y ( k)) + ( z ( k) z ( k)) + ε, (10) obs where the cost functon s chosen to decay unformly n every drecton from the obstacle pont at ( xmn, ymn, z mn ). K obs s a tunng parameter to balance the tendency to stay on the orgnal gven path and to break away from the gven path to go around obstacles. B. ocal bstacle Map Buldng For the MPC-based trajectory generaton wth obstacle avodance, we need to fnd mnmum length from the reference poston to a pont on an obstacle such that mn, the relatve vector wth ( ) = arg mn, (11) mn ref ref obs where s Eucldan norm n three-dmensonal space and obs s the set of all ponts on the obstacles n the surroundng three-dmensonal space. Theoretcally, Eq. (11) demands a perfect knowledge on all obstacles n the surroundng envronment, whch would requre an deal sensor capable of omn-drectonal scannng wth nfnte detecton range. Further, f the MPC s for reference trajectory generaton, the deal sensor should be movng along the trajectory of the reference ponts durng the state propagaton over a fnte horzon at every optmzaton stage. bvously, such a scenaro s mpossble to realze. mn / B Fgure 4. Nearest-pont method 5 Amercan Insttute of Aeronautcs and Astronautcs

6 Therefore, n order to provde mn to the MPC-based trajectory generator along the reference trajectory, t s necessary to mantan a local obstacle map consstng of recent measurements from onboard sensors. At each sample tme, the sensor provdes N meas measurements of scan ponts on obstacles nearby. Due to the mperfect coverage of the surroundngs wth possble measurement errors, each measurement set s frst fltered, transformed nto local Cartesan coordnates, and cached n the local obstacle map. A frst-n frst-out (FIF) buffer s chosen as the data structure for the local map, whose update rate depends on the types of obstacles nearby. If the surroundng s known to be statc, the cachng tme s desred to be as long as the memory and processng overhead permts. n the other hand, a more dynamc envronment would requre shorter cachng to reduce the chance to detect obstacles that may not exst any more. Raw scan data from aser canner Fnd Nearest obstacle pt to MPC Flter out naccurate measurements ort and flter out small obstacles In order to solve Eq. (11), the measurement set n the FIF s sorted n ascendng order of for all n the local obstacle map. Pror to be regstered n the database, any anomales such as salt-and-pepper nose are dscarded. Then, the measurements are examned for small debrs, such as grass blades or leaves blown by the vehcle. uch small-sze objects, not beng serous threats for safety, s also gnored. We apply an algorthm that computes a boundng box of the mnmum volume that contans a seres of subsequent ponts n FIF where the dstance between adjacent ponts n the sorted sequence s less than a predefned length. Then, f the volume of the box s larger than a threshold of becomng a threat, the coordnates of the nearest pont n the boundng box s found and used for computng Eq. (3). The procedure of the local obstacle map buldng method proposed above s llustrated n Fgure 5. Vehcle poston and atttude from Host vehcle Convert to local cartesan coordnates Fnd N1 ponts closest to host vehcle Fgure 5. ocal partal map buldng method for nearest-pont approach usng MPC ref IV. Experment Results In ths secton, we present the smulaton and experment results of autonomous exploraton n an urban envronment. The experment desgn s carefully scrutnzed for the safety regulatons: t s performed n a feld wth portable canopes smulatng buldngs, not wth real buldngs. The canopes, measurng meters each, are arranged as shown n Fgure 6. The dstance between one sde to the next adjacent sde of canopes s set to 10 meters n the north-south drecton and 1 meters n the east-west drecton so that the UAV wth 3.5 meter long Fgure 6. Aeral vew of urban experment (black: gven straght path, red: actual flght path of UAV durng experment) Fgure 7. mulaton of MPC-based path plannng n the proposed urban experment setup 6 Amercan Insttute of Aeronautcs and Astronautcs

7 fuselage can pass between the canopes wth mnmal safe clearance, about 3 meters from the rotor tp to the nearby canopy when stayng on course. For valdaton, the MPC engne developed n Ref. 4 s appled to the proposed urban experment setup. A smulaton model s constructed n MATAB TM /mulnk TM, where the local map buldng wth a laser scanner s substtuted wth a pre-programmed map. The MPC wth the local map buldng algorthm s mplemented n C language for speed and portablty. As shown n Fgure 7, the MPC path planner s capable of generatng a collson-free trajectory around the buldngs based on the orgnal trajectory wth ntentonal ntersectons wth buldngs. The green and red lnes pontng to the buldngs represent mn computed at each sample tme. For experments, the mulnk model s modfed to functon as the onlne trajectory generator. Although mulnk was not desgned for a real-tme controller n the loop, t can be forced to run for soft real-tme control purpose by addng a real-tme enforcng block: usng the behavor of TCP/IP communcaton, a custom TCP/IP transport block s confgured to enforce soft real-tme operaton of the mulnk model at 10Hz n ths case. Urban exploraton experments were performed usng a Berkeley UAV as shown n Fgure 1, whose detaled specfcaton s gven n Table 1. For obstacle detecton, the vehcle s equpped wth an M-00 from ck AG (Fgure 3), a two-dmensonal laser range fnder. It s capable of 80 m scannng range wth 10 mm resoluton and weghs 4.5 kg. The measurement s sent to the flght computer va R-3 and then relayed to the ground staton runnng the MPC-based trajectory generator n mulnk. The laser scanner data s then processed followng the method proposed n ecton III-B. In Fgure 8, a three-dmensonal renderng from the ground staton software s presented. The dsplay shows the locaton of the UAV, the reference pont marker, Table 1. pecfcaton of a Berkeley UAV Base platform Yamaha R-50 Industral Helcopter Dmenson 0.7 m(w) 3.5 m () 1.08 m (H) Rotor Dameter m 44 kg (dry weght) Weght 0 kg (payload ncludng avoncs) cycle gasolne engne Engne 1 hp Fuel: 40 mnutes peraton Tme Avoncs: 00 mnutes Two PC104-based computers Boeng DQI-NP IN NovAtel GP MllenRT- nboard ystem IEEE 80.11b Wreless Ethernet Ultrasonc altmeters ICK laser range fnder (M-00) Waypont navgaton Capabltes Poston trackng Interactve operaton mode to a pont n the local obstacle map at that moment, and laser-scanned ponts as blue dots. Durng the experments, the laser scanner used n our experment demonstrated ts capablty to detect the canopes n the lne of sght wth great accuracy, as well mn / B Canopy n preprogrammed map aser scan ponts from canopy mn / B UAV reference poston marker Fgure 8. A snapshot of three-dmensonal renderng durng an urban exploraton experment 7 Amercan Insttute of Aeronautcs and Astronautcs

8 as other surroundng natural and artfcal objects ncludng buldngs, trees and power lnes. The processed laser scanned data n a form of local obstacle map s used n the optmzaton Eq. (10) to generate a trajectory usng the algorthm n ecton III-A. The trajectory s then sent va IEEE 80.11b to the avoncs system wth a dedcated process runnng to enforce the command update rate at 10Hz. The trackng control layer enables the host vehcle to follow the gven trajectory wth suffcent dstance. In the repeated experments, the vehcle was able to fly around the obstacles wth suffcent accuracy for trackng the gven trajectory, as shown n Fgure 6 (red lne). V. Concluson Ths paper presented an obstacle avodance and local map buldng method for unmanned aeral vehcles to explore unknown urban envronments. An onboard laser scanner s used to buld an onlne map of obstacles around the vehcle. Ths local map s combned wth a real-tme MPC algorthm that generates a safe vehcle path, usng a cost functon that penalzes the proxmty to the nearest obstacle. The adjusted trajectory s then sent to a poston trackng layer n the herarchcal UAV avoncs archtecture 6. In a seres of experments usng a Berkeley UAV, the proposed approach successfully guded the vehcle safely through the urban canyon. VI. Acknowledgment Ths research was supported by DARPA (F C-3614) and AR MURI (DAAD ). References 1. Thrun. Robotc mappng: A survey. In G. akemeyer and B. Nebel, edtors, Explorng Artfcal Intellgence n the New Mllenum. Morgan Kaufmann, 00. M. C. Martn and H. P. Moravec, Robot Evdence Grds, CMU Robotcs Insttute Techncal Report, CMU-RI- TR , Mar G. J. utton and R. R. Btmead, Computatonal Implementaton of NMPC to Nonlnear ubmarne, n F. Allgöwer and A. Zheng, edtors, Nonlnear Model Predctve Control, volume 6, pages , Brkhäuser, D. H. hm, H. J. Km, and. astry, Decentralzed Nonlnear Model Predctve Control of Multple Flyng Robots, IEEE Conference on Decson and Control, Mau, HI, December J. prnkle, J. M. Eklund, H. J. Km and. astry, Encodng Aeral Pursut/Evason Games wth Fxed Wng Arcraft nto a Nonlnear Model Predctve Trackng Controller, pages , 43rd IEEE Conference on Decson and Control, Paradse Island, Bahamas, December, D. H. hm, Herarchcal Control ystem ynthess for Rotorcraft-based Unmanned Aeral Vehcles, Ph. D. thess, Unversty of Calforna, Berkeley, Amercan Insttute of Aeronautcs and Astronautcs