4DoF Drift Free Navigation Using Inertial Cues and Optical Flow

Transcription

1 213 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems (IROS) November 3-7, 213. Tokyo, Japan 4DoF Drft Free Navgaton Usng Inertal Cues and Optcal Flow Stephan Wess, Roland Brockers, Larry Matthes Jet Propulson Laboratory, Calforna Insttute of Technology (roland.brockers, Abstract In ths paper, we descrbe a novel approach n fusng optcal flow wth nertal cues (3D acceleraton and 3D angular veloctes) n order to navgate a Mcro Aeral Vehcle (MAV) drft free n 4DoF and metrc velocty. Our approach only requres two consecutve mages wth a mnmum of three feature matches. It does not requre any (pont) map nor any type of feature hstory. Thus t s an nherently falsafe approach that s mmune to map and feature-track falures. Wth these mnmal requrements we show n real experments that the system s able to navgate drft free n all angles ncludng yaw, n one metrc poston axs, and n 3D metrc velocty. Furthermore, t s a power-on-and-go system able to onlne selfcalbrate the nertal bases, the vsual scale and the full 6DoF extrnsc transformaton parameters between camera and IMU. I. INTRODUCTION AND RELATED WORK Mcro Aeral Vehcles (MAVs) have seen an ncreased popularty n recent years due to a wde range of new applcatons n reconnassance, survellance, search and rescue, and envronmental montorng. In partcular, multcopter systems (e.g. quadrotors) have a dstnct advantage n ther hoverng capabltes and aglty to counteract strong wnds. Ths aglty, however, comes at a cost: quadrotors are nherently unstable n flght and requre good state estmaton and control to mantan a poston or to perform defned maneuvers. Navgatng such a system s partcularly challengng snce there s no hold functon as compared to ground vehcles that can be entered as a safety regme (e.g. holdng all actuators wll not result n zero velocty but n a crash). Therefor, mult-rotor MAV requre constant and accurate state estmaton and control. Furthermore, a msalgnment to gravty not only results n a velocty vector but n an acceleraton. Smlarly, smply ntegratng IMU acceleratons for poston hold wll result n a crash due to nose and bas terms on the accelerometers. A mnmal requrement to keep a quadrotor arborne s to have a contnuous metrc velocty 1 and a precse gravty algned atttude estmate. The MAV may stll drft n poston and yaw, however, gven an obstacle free area, the vehcle remans arborne. There s a large body of work for ndoor MAV control usng moton trackng systems [1], [2], [3] and for outdoor operatons usng GPS sgnals [4], [5]. In contrast to these approaches that depend on external postonng nformaton, Ths research was carred out at the Jet Propulson Laboratory, Calforna Insttute of Technology, under a contract wth the Natonal Aeronautcs and Space Admnstraton. 1 f the velocty estmate s not metrc, the controller cannot tell f the vehcle drfts wth 1 m s or λ 1 m. Ths results n oscllaton and nstablty. s ths paper focuses on MAV control n envronments where an external trackng setup s nfeasble (e.g. large outdoor areas) and GPS sgnals may be corrupted or unavalable (e.g. n ctes, caves etc.). A. Control and Navgaton Based on Maps A popular approach s to control and navgate MAVs based on maps wthout the need of a moton capturng system or GPS. Ths s often done usng known markers, pre-bult maps or maps bult on the fly usng SLAM or keyframe based vsual odometry (VO) technques. Such approaches usually control the vehcle n ts 6DoF pose (poston and atttude). The drft n poston and yaw s ether very low or even elmnated by usng known, world-fx structures. Sensors that are commonly used for map generaton are laser scanners [6], [7] or cameras ncorporatng known markers [8], [9] or SLAM/VO technques [1], [11]. Snce laser scanners are stll too heavy and power hungry for small quadrotor systems, our work focuses on vson based approaches. Common to all approaches usng a map for moton estmaton s, that the map can get corrupted or lost. In such a case recovery s dffcult f not mpossble and the vehcle s prone to crash. B. Control and Navgaton Wthout Maps and Feature Hstory In order to avod the ssue of a map loss, we follow the approach of not havng any type of feature hstory except the feature matches between two consecutve mages:.e. optcal flow (OF). A hstory free approach augments the algorthms robustness due to the ndependence on past readngs. OF approaches wthout ncludng 6D nertal measurements are presented n [12], [13]. Whle already showng the capabltes of OF for MAV navgaton, these approaches act on a reactve manner to keep the vehcle away from ground or from obstacles. Reactve control to poston-keep or trajectory-navgate a mcro ar vehcle s not suffcent snce the unavalablty of metrc nformaton can result n nstablty of these nherently unstable systems. More recent work ncludes 6D nertal readng and successfully estmates not only the metrc velocty [14], [15] but also nertal bases nter-sensor transformatons and a gravty algned atttude of the MAV [16] - the mnmal requrements n order to keep a MAV arborne /13/$ IEEE 418

2 C. Full Informaton Acquston for Complete State Estmaton Our work s a contnuaton of prevous work n [16] wth the novelty that we use the full nformaton provded by OF and nertal readngs to acheve a complete vehcle state estmaton not dscardng any nformaton. The man contrbuton of ths work s to show n real-world tests that: we can estmate the metrc dstance to the over-flown terran usng only OF (.e. only two consecutve mages) and nertal readngs. Ths drectly leads to robust, metrc terran-followng capabltes. wth OF and nertal readngs only, t s addtonally possble to estmate the terran nclnaton towards gravty. Ths leads to the elmnaton of the global yaw drft and enables the MAV to navgate n 3D terran. a concse analyss for the vsual scale propagaton n the Extended Kalman Flter (EKF) framework leads to mproved results compared to our prevous. Compared to commercal products, our approach does not requre an actve sensor (e.g. ultra sonc altmeter). Ths reduces the weght and power consumpton of the overall sensor sute. More mportant, our approach does not requre a gravty algned ground plane for theoretcally correct functonng. In fact, our approach gans on performance n nclned terran as explaned later. The remander of the paper s organzed as follows. In Secton II we brefly descrbe the computaton of the optcal measurements used later n the EKF framework. These measurements are the arbtrarly scaled 3D camera velocty vector and the terran plane-parameters n the camera frame. Secton III descrbes our EKF framework showng the capablty of addtonally estmatng the terran nclnaton and metrc terran dstance to the MAV. We also dscuss here the novel approach for the vsual scale propagaton wthn the EKF. Secton IV presents the real world results to show the functonng of the proposed approach. We conclude the paper n Secton V. II. COMPUTATION OF OPTICAL MEASUREMENTS Ths secton s dedcated to the theoretcal background on how to compute the scaled vsual 3D velocty vector, the terran nclnaton as perceved n the camera frame and the scaled dstance from the camera to the terran. These measurements wll be used n the next secton as an update step of the EKF framework estmatng all states of the system and the terran. As n our prevous work [16] we make use of the contnuous eppolar constrant and the addtonal nformaton of the angular veloctes measured by the Inertal Measurement Unt (IMU) to compute the optcal measurements. A. 3D Velocty Computaton up to Unfed Scale As descrbed n [17], the contnuous moton of a feature wth respect to the camera s Ẋ(t) = ω(t) X(t) + V (t) (1) Wth X beng the 3D feature poston, V the camera velocty, and ω(t) the skew symmetrc matrx of the camera angular veloctes. The feature scale factor λ represents the optcal flow of a feature pont wth X = λ x, where x s the unt length feature drecton vector. Together wth ts dervatve, we get the contnuous eppolar constrant [17]: x T v(t) x + x T ω(t) v(t) x = (2) Smlar to the feature scale factor λ we apply a velocty scale factor η wth V = η v n the above equaton. Unrotatng the optcal flow wth the angular veloctes from the IMU elmnates the second term n (2) and reduces the problem to ( x(t) x) T v = (3) Ths equaton can be solved for v usng N features and SVD. Note, that the complexty of the SVD s only of dmenson three (for the 3D velocty vector v). Snce any scaled verson of the vector v solves (3), we can chose t to be of unt length wthout loss of generalty. As suggested n [17] and from (1) wth ω = usng the IMU any (un-rotated) feature used n (3) needs to fulfll ts moton equaton λ (t) x (t) + λ (t) x (t) = η v(t) (4) Whereas the scale factor λ s dfferent for every feature (reflectng the 3D structure of the terran), the velocty scale factor η s the same for all features. When stackng all λ, λ, and η nto the vector λ = [λ1,..., λ n, λ 1,..., λ n, η] (5) (4) can be rephrased as the SVD problem M( x, x, v) λ = (6) The soluton λ unfes all scale factors n a consstent way up to one common scale factor Λ. Ths s essentally a contnuous trangulaton of feature postons snce we can reconstruct the 3D scene up to common scale Λ wth X metrc Λ = λ x (7) We note at ths pont, that the scene can be of any 3D structure and that the algorthm s not bound to planar terran. Smlar to (3) the soluton of the SVD n (6) can be arbtrarly scaled. We frst ntroduce the noton of the terranplane and the computaton of ts parameters. Then we propose a specfc normalzaton of the above velocty scale factor such that we are able to defne analytcally the dynamcs of the common scale Λ later n the EKF framework. Such an analytc descrpton mproves the scale state estmate n the EKF predcton step. 4181

3 B. Terran-plane Parameter-Computaton The descrbed scene reconstructon wth X = λ x yelds a 3D pont cloud representng the 3D structure of the scene n the camera frame. We defne the term terran-plane as the plane ftted to ths pont cloud by the regresson [ n T tp, d tp ][λ x T, 1] T = (8) wth the normal vector n tp and the dstance to the orgn (.e. camera center) d tp. Furthermore, we denote a terran as locally reasonably flat f the regresson model of the terran-plane s locally constant n a fxed world frame (Fg. 1). Sngle outler objects (e.g. a sngle tree) can easly be accounted for usng robust outler rejecton methods durng the plane fttng process. Features on such objects, however, are stll used for the above velocty computaton. Note, that the terran s assumed to be locally flat wth respect to the vehcle dynamcs. For agle MAVs, ths assumpton holds for a large varety of outdoor terrans. Fg. 1. A shows a locally reasonable flat terran. That s, when the camera (black trangle) moves and observes a dfferent part of the terran (black lne), the plane parameters n the world frame based on the regresson of the trangulated features (red dots) stll reman constant. The terran-plane s depcted as dashed cyan lnes. In B, the plane parameters change when the camera moves, whch volates the assumpton of a constant terran-plane n the world frame. Fg. 2 vsualzes the dfferent values computed usng only the contnuous eppolar constrant and optcal flow of two consecutve mages. constant or to know ts contnuous moton model. In [16] we assumed a hoverng MAV wth a camera always pontng strctly down and normalzed the soluton vector λ of (6) wth respect to all feature scale factors λ. In practce, the MAV rolls and ptches wth respect to the terran-plane and never s exactly hoverng. Usng optcal flow cues for computng the terran-plane parameters, has the advantage of provdng the dstance d tp between the camera and the terran-plane. Ths dstance can be used as a normalzaton factor to keep the common scale factor Λ constant even f the camera atttude wth respect to the terran-plane changes. Wth ths normalzaton, we can calculate the exact moton model for the common scale Λ n the EKF propagaton step. The normalzaton λn = λ d tp (9) renders the change of the common scale Λ nverse proportonal to the change of the dstance between the camera and the terran-plane. As we wll see n the next secton, ths dstance can be computed by only usng state varables of the EKF state vector. Ths makes t possble to have an exact propagaton model of Λ n the EKF propagaton phase. III. EXTENDED KALMAN FILTER In the prevous secton, we showed how to compute the followng vsual cues whch have an arbtrary but common scale factor Λ to metrc values: 3D camera velocty vector (same as n [16]) terran plane parameters: normal vector and dstance of the camera to the plane (novel contrbuton) We used a novel normalzaton approach such that we wll be able to express the dynamcs of Λ n the propagaton step of an EKF framework usng the state vector and nertal system nputs. The computed vsual cues are then used as measurements n the EKF update step. We focus on our contrbutons of the terran plane representaton and the propagaton of Λ and summarze the remanng EKF parts as standard approaches. We assume that the IMU nputs have the followng model wth bas b and zero mean whte Gaussan nose n. We denote the accelerometer model wth subscrpt a and the gyroscope model wth subscrpt ω: Fg. 2. We summarze that the features on the terran can be reconstructed n 3D usng ther specfc scale factor and ther drecton vector λ x up to a metrc scale factor Λ. A planar regresson of these features yelds the terran plane whch s characterzed wth ts normal vector n tp and ts (scaled) dstance to the camera d tp. On the rght, the computaton of the optcal flow x usng two consecutve camera frames s llustrated. C. Scene Depth Normalzaton From (6) we know that the body-velocty and terran plane dstance computaton are only correct up to a common scale factor Λ. For the later estmaton of Λ n an EKF framework wth added nertal cues, t s hghly desrable to have Λ ω = ω m b ω n ω, a = a m b a n a (1) b ω = n bω, b a = n ba (11) The state vector χ = {p w v w q w b ω b a Λ p c q c α} (12) contans the IMU-centered MAV poston p w, velocty vw and atttude qw wth respect to the world frame. It also contans the IMU bases on gyroscopes b ω and accelerometers b a, the common vsual scale factor Λ and the 6D transformaton between the IMU and the camera 4182

4 n translaton p c and rotaton qc. Thus, we provde a selfcalbratng and so-called power-on-and-go system. A plane the terran plane n ths case s represented usng three parameters: two for the unt normal vector (elevaton α and azmuth β) and one for the dstance of the plane to the orgn. Intuton and a thorough non-lnear observablty analyss usng dfferental geometry as proposed n [18] reveal that the azmuth β and the dstance of the plane to the orgn are unobservable states. Intutvely, only havng a vsual body velocty and the plane parameters n the floatng camera frame as measurements does not anchor the system to a fx world frame n poston and yaw (gravty ncluded n the IMU readngs anchor the system n global roll and ptch). Hence, the dstance of the terran plane to the world orgn and the system s poston p w are only jontly observable. So are the systems yaw n q w and the terran plane s azmuth. If two states are jontly observable t s suffcent to provde a measurement for one of them to render the other observable. In our case, we assume that we operate n locally reasonably flat terran whch keeps the terran plane locally constant n the world frame. Hence, wthout loss of generalty, we can anchor the world frame n the terran plane such that ts dstance to the orgn and azmuth vanshes. Ths drectly renders the system s global yaw and the system s postondmenson perpendcular to the terran plane observable. Fg. 3 depcts the setup and frame algnment. A. System Dynamcs The followng dfferental equatons govern the state χ: ṗ w = v w (13) v w = C T (q w )(a m b a n a ) g (14) q w = 1 2 Ω(ω m b ω n ω )q w (15) ḃ ω = n bω, ḃ a = n ba, p c =, p c =, α =, (16) where C (q) s the rotatonal matrx correspondng to the quaternon q, g s the gravty vector n the world frame, and Ω(ω) s the quaternon multplcaton matrx of ω. The transformaton between camera and IMU (p w, qw) and the terran-plane-vector elevaton α are assumed not to change. We desgn the flter n ts error state for mnmal representaton and better handlng of the quaternons [19]. Rather than detalng the standard procedure for the error representaton, we focus on the dynamcs of the common scale factor Λ. We could assume a random walk of Λ snce t changes on every optcal flow readng. However, due to our specfc normalzaton n the prevous secton, we can express the dynamcs of Λ analytcally, yeldng mproved results compared to the random walk assumpton. We recall that the normalzaton factor used n the prevous secton was the dstance d tp from the camera to the terran plane (9). We can express the metrc representaton of d tp usng the states of the system by frst expressng the camera poston n the world frame and then projectng ths poston along the plane normal vector n wtp n the world frame n order to get the metrc dstance d wtp Fg. 3. Frame setup and state defnton for the EKF framework. Wthout loss of generalty, we can lock the gravty algned world y-axs along the terran plane. The terran plane normal vector can then be descrbed as n wtp = [cos(α) sn(α)] T n the world frame. The red values are states estmated n the EKF framework, whereas the blue values are the scaled vsual measurements normalzed wth our proposed approach ad of the terran-plane. As a result, the MAV can not only keep ts metrc dstance to the terran but also keep ts full atttude algned n roll and ptch wth gravty and n yaw wth respect to the terran plane. Ths allows advanced terran followng mssons n large envronments. We note at ths pont that we stll use optcal flow measurements (.e. two consecutve mages) and the correspondng IMU readngs only. We do not use any temporal hstory of features or states. The MAV, of course, wll drft n the poston-dmensons parallel to the terran plane. d wtp = (p w + C T (q w )pc ) T n wtp (17) The camera poston n the world frame s the IMU poston n the world frame p w plus the IMU-atttude (qw) dependent translaton between camera and IMU p c expressed n the IMU frame. Snce n wtp s a unt vector and the world frame s anchored n the terran plane up to an elevaton angle and arbtrary azmuth, we can wrte n wtp = [cos(α) sn(α)] T. d tp beng the normalzaton vector, the vsual dstance measurement between camera and terran plane wll always be dn tp = dtp = 1. Furthermore, we defned n (7) the d tp common scale as p vson = Λp metrc Thus, the further away the camera moves from the plane the smaller becomes the common scale factor Λ. More precsely, f the MAV doubles ts dstance, Λ wll be cut n half. In other words, the change n percent of the common scale s nversely proportonal to the change n percent of the dstance of the camera to the terran plane. The change of the metrc dstance d wtp s d wtp = (ṗ w) T n wtp + (p w) T ṅ(α) wtp α + (p c )T C (q w )n wtp + (p c ) T Ċ (q w )n wtp +(p c ) T C (q w )ṅ wtp α = (v w) T n wtp + (p c ) T Ċ (q w )n wtp (18) 4183

5 The change n percent s then d p w tp = d wtp d wtp (19) Invertng ths change leads to the change of the common scale factor per tme-step Λ t Λ t+dt = 1 + (2) t+dt d p t w tp Fg. 4 shows the correlaton between the scale change n percent and the nverted camera-to-terran-plane dstance change n percent. change n % scale change n % nv. camera-to-plane dstance change n % smulaton tme Fg. 4. Correlaton between the scale change n percent and the nverted camera-to-terran-plane dstance change n percent. Thanks to our novel approach n computng the terran plane parameters ad of optcal flow, we can normalze the optcal flow based body velocty readngs wth respect to the plane dstance. Ths allows to have a vsual scale change nverse proportonal to the plane dstance - and n turn ths dstance can be represented by the systems state allowng a precse scale predcton step. The graph s computed based on nosy smulaton data. We compare the ablty to correctly propagate the vsual scale factor versus the assumpton of the scale factor beng a random walk as done n [16]. Naturally, the scale-change depends on the camera poston wth respect to the scene and never reflects an arbtrary random walk. Fg. 5(a) shows the EKF-estmated versus true scale factor Λ by applyng a random walk as dynamc model of the scale. We note that for ths result, we hand-tuned the nose parameter of the random walk to obtan the best result possble. In a dfferent run ths nose parameter would change due to the false assumpton of the scale beng a random walk. Fg. 5(b) shows the same smulaton run but usng our dynamc model for the scale propagaton n the EKF. Even though the performance only margnally mproves (below 1%) we note that n ths case, there are no hand-tunable parameters for the scale propagaton and the model s vald for any dfferent moton. It s nterestng that n both cases we have constantly an under-estmaton of about 5% wth respect to the true scale. Ths s subject to further nvestgaton. B. EKF Measurements The EKF system has three dfferent measurements: the scaled camera body velocty z v = η v, the terran-plane normal vector n the camera frame z n = n tp and the dstance from the camera center to the terran plane z d = d ntp = 1. The measurement equatons are: ẑ v = (C (q c )C (q w )v w + C (q c )( ω p c ))Λ + n zv (21) ẑ n = C (q c )C (q w )n wtp (α) + n zn (22) ẑ d = (p w + C T (q w ) pc ) T n wtp (α)λ + n zd (23) scale (random walk) EKF true smulaton tme (a) scale (propagated) EKF true smulaton tme Fg. 5. a) Performance of the EKF estmate for the vsual scale factor Λ when assumng a random walk as a moton model and b) when usng our proposed moton model. The nose of the random walk s hand-tuned to obtan the best possble result. The value of ths parameter s not vald anymore for a dfferent smulaton run snce the moton wll be dfferent. Conversely, our proposed model s vald for all motons. Note the step-wse behavor n a) versus the smooth, more accurate propagaton n b). where C (q w ) and C (q c ) s the atttude of the IMU and the rotaton between the IMU and camera respectvely. All these measurements are results of the optcal measurements based on solutons of SVD problems. We can use the approach of the mean squared error (MSE) n statstcs to determne the accuracy of the SVD soluton. Thus, the covarance of the nose vector n z = [n zv n zn n zd ] can be computed along wth the optcal measurements n Secton II. The above measurement equatons can be lnearzed wth respect to the state vector χ and stacked to the measurement vector z = [z v z n z d ] T whch yelds ẑ = Hχ. Then, the standard EKF procedure can be appled 1) compute the resdual r = z ẑ 2) compute the nnovaton S = HP H T + R 3) compute the Kalman gan K = P H T S 1 4) compute the correcton ˆχ = Kr 5) update the covarance matrx P k+1 k+1 = (I d KH)P k+1 k (I d KH) T + KRK T. Wth ths EKF framework we acheved the followng: fully contnuous self-calbratng platform ncludng IMU bases, vsual scale and nter-sensor transformaton between IMU and camera terran plane algned observable global yaw and metrc dstance to ths plane effectvely elmnatng drft n the poston-dmenson vertcal to the plane precse vsual scale propagaton such that the system can track even fast scale-changng motons of the MAV Whle the frst pont was already fulflled n related work, the second and thrd ponts only became possble wth our contrbuton of computng the terran plane parameters usng the optcal flow cues. Frst, lockng the world frame onto ths terran plane rendered global yaw and metrc dstance wth respect to the terran observable. Second, the scaled vsual dstance measurement to the plane allowed to normalze the vsual readngs such that the change of the common scale factor Λ can be recovered usng system states. (b) IV. PERFORMANCE EVALUATION Ths secton dscusses the performance of the proposed system n a real-world scenaro. We mplemented our approach on-board an AscTec Pelcan quadrotor that s equpped wth an Intel Atom 1.6GHz processng board and a global shutter WVGA camera (Fg. 6) usng ROS for nter 4184

6 process communcaton. Even though our approach would run at 5Hz on ths platform, for the followng experments, we ran the camera at 3Hz and the IMU at 1kHz to provde a safety margn. The exctaton of the system has an RMS value of.5m/s 2 n acceleraton and.25rad/s n angular veloctes. global yaw [rad] EKF estmate ground truth Fg. 8. Global yaw estmated by our proposed approach. The observed terran has an nclnaton of 6 degrees. After a wrong ntalzaton, the flter quckly converges to the correct yaw and remans there wthout drft. Fg. 6. AscTec pelcan equpped wth a 1.6GHz Intel Atom sngle core processor-board and a WVGA global shutter camera. elev. angle [deg] 2 EKF estmate ground truth Fg. 9. After a wrong ntalzaton, our proposed approach converges to the true elevaton angle of the normal vector of the observed terran. Note that the framework estmates the elevaton angle of the plane normal vector, not the plane nclnaton. Hence the correct 3deg for a 6deg nclned plane. Whle the evaluaton of the self-calbraton and the metrc velocty estmaton was provded n [16], we focus on the novel contrbuton of estmatng the terran parameters and absolute yaw. Fg. 7 shows schematcally the setup. For our experments we used textured styrofoam plates mounted rgdly at hand measured angles of 4 and 6 degrees respectvely. The MAV moves at a dstance of 1m to 1.5m above these planes. Fg. 7. Test setup to evaluate our approach. We use two dfferently nclned terran planes: textured styrofoam plates, 4 and 6 degrees nclnaton, manually measured. The MAV was moved across the two planes to show the dfferent behavors for dfferent plane nclnatons. We frst show the capablty of the real system to mantan global yaw. The normal vector of an nclned terranplane computed n the camera frame (Secton II) contans a component perpendcular to the gravty vector, whch s n fact the nformaton that renders global yaw observable n our proposed system. In Fg. 8 we show the evoluton of the estmate n global yaw durng 8 where the MAV s camera s observng a terran wth an nclnaton of 6 degrees. Fg. 9 shows the estmated elevaton angle of the plane normal vector. The ground truth for yaw was obtaned from the MAV magnetometer wth ntal algnment. In Fg. 1 we compare the qualty of ths estmate for two dfferent setups, one n whch the terran has an nclnaton of about 6 degrees and one wth an nclnaton of about 4 degrees. In both graphs we see that the system converges to the correct yaw, however, the terran whch s nclned by 4 degrees provdes much less drectonal nformaton perpendcular to gravty. Hence, global yaw s less constraned. A front lookng camera lookng at vertcal walls would yeld best results. In addton to the yaw, roll and ptch are also observable (c.f. our prevous work n [16]). Hence our approach s able to estmate the full 3DoF atttude of the MAV wthout drft. Next, we show that we can mantan the dstance to the terran. By rotatng the estmated poston from our proposed EKF framework to the terran frame usng the estmated nclnaton angle α, we expect drft n both drectons x and y, whereas z remans constant. In the terran frame, the z axs represents the drecton of dstance vector between between the MAV and the terran. The estmated z-poston can drectly be used for terran-followng controllers. Fg. 11 shows the expected results over a terran wth an nclnaton of 6 degrees. Note that thanks to the vsual scale estmaton n the EKF framework, the vehcle keeps a constant metrc dstance to the terran. In Secton II we stated that our approach for estmatng the terran parameters requres locally reasonably flat terran. Fg. 12 shows that our approach can quckly adapt to Fg. 1. The qualty of the yaw estmaton depends on the nclnaton of the terran. The steeper the terran s, the more s ts normal vector perpendcular to gravty and the better can global yaw be estmated. The graphs show the yaw estmaton over terrans wth an nclnaton of 6 and 4 degrees respectvely. The ntal large error n the top graph s due to an offset to the true value n the ntalzaton. The yaw remans unobservable on flat terran and s best observable wth a front lookng camera lookng at walls. Ths s a well desred aspect for example n nspecton tasks. 4185

7 changng scenes, even when ths requrement s not met. Quadrotors usually have suffcent moton to make the terran parameters quckly converge to the new stuaton. In our experment, the vehcle frst observes a terran wth an nclnaton of 6 degrees (ths corresponds to an elevaton angle of 3 degrees), then the terran changes to an nclnaton of 4 degrees (elevaton angle s 5 degrees). poston drft [m] EKF drft x EKF drft y EKF drft z Fg. 11. We rotate the estmated global poston nto the terran frame wth z beng n the drecton of the terran normal vector. Snce we compute ths scaled value and estmate the scale n the EKF framework the MAV s able to mantan ths metrc dstance constant. Ths s crucal for terran followng tasks. x and y drecton drft, as expected. elev. angle [deg] EKF estmate terran 1 terran Fg. 12. Even though our approach requres locally reasonably flat terran, ths plot shows that our approach can quckly adapt to terrans wth dfferent nclnatons. In ths case, the terran swtched from a 6 degree to a 4 degree nclnaton after 1 of flght. The plane normal vectors have an elevaton of 3 and 5 degrees respectvely. V. CONCLUSION In ths work, we present an nertal-optcal flow approach whch sgnfcantly advances the state-of-the-art wth respect to drft free MAV navgaton and terran followng. Based on the ablty to estmate metrc body veloctes wth optcal flow and nertal readngs, our approach uses the defnton of the contnuous eppolar constrant to compute the terran parameters and the metrc dstance between the vehcle and the terran. We motvate to use the latter as vsual normalzaton factor n order to express the dynamcs of the vsual scale analytcally durng the EKF predcton step. Estmatng the metrc dstance to the terran plane also provdes the key-nformaton for a controller performng terran followng. Even though we make the assumpton of navgatng n locally reasonable flat areas, we show n real experments that the system can quckly adapt to new terran parameters and s thus capable of robustly follow dfferent terran. Lockng the world frame to the terran and estmatng the terran nclnaton to gravty made global yaw of the system observable. Also, the metrc dstance to the plane elmnates the system s poston drft n one dmenson. In addton, the results show that the drft n the other two poston axes s mnmal. We demonstrate n real experments that the qualty of the yaw s dependent on the terran nclnaton. Ths reflects the nfluence of the perpendcular component to gravty on the estmaton qualty. Our proposed approach yelds a state estmaton that s drft free n 4DoF (poston drfts only parallel to the terran plane) whle only usng nertal readngs and two consecutve mages (.e. optcal flow) wthout any map or feature hstory. REFERENCES [1] N. Mchael, J. Fnk, and V. Kumar, Cooperatve manpulaton and transportaton wth aeral robots, Autonomous Robots, 21. [2] N. Mchael, D. Mellnger, Q. Lndsey, and V. Kumar, The grasp multple mcro UAV testbed, IEEE Robotcs and Automaton Magazne, May 21. [3] S. Lupashn, A. Schoellg, M. Sherback, and R. D Andrea, A smple learnng strategy for hgh-speed quadrocopter mult-flps, n Internatonal Conference on Robotcs and Automaton, 21. [4] N. Abdelkrm, N. Aouf, A. Tsourdos, and B. Whte, Robust nonlnear flterng for INS/GPS UAV localzaton, n Proceedngs of the 16th IEEE Medterranean Conference on Control and Automaton, Ajacco, Corsca, France, 28, pp [5] B. Yun, K. Peng, and B. Chen, Enhancement of GPS sgnals for automatc control of a UAV helcopter system, n Proceedngs of the IEEE Internatonal Conference on Control and Automaton, Hong Kong, Chna, 27, pp [6] A. Bachrach, R. He, and N. Roy, Autonomous flght n unstructured and unknown ndoor envronments, n European Conference on Mcro Aeral Vehcles (EMAV), 29. [7] S. Shen, N. Mchael, and V. Kumar, Autonomous mult-floor ndoor navgaton wth a computatonally constraned MAV, n Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 211. [8] T. Chevron, T. Hamel, R. Mahony, and G. Baldwn, Robust nonlnear fuson of nertal and vsual data for poston, velocty and atttude estmaton of UAV, n Internatonal Conference on Robotcs and Automaton, Apr. 27, pp [9] K. E. Wenzel, A. Massell,, and A. Zell, Automatc take off, trackng and landng of a mnature UAV on a movng carrer vehcle, n UAV 1 3rd Internatonal Symposum on Unmanned Aeral Vehcles, 21. [1] S. Ahrens, D. Levne, G. Andrews, and J. How, Vson-based gudance and control of a hoverng vehcle n unknown, GPS-dened envronments, n Internatonal Conference on Robotcs and Automaton, May 29. [11] S. Wess, D. Scaramuzza, and R. Segwart, Monocular-SLAM based navgaton for autonomous mcro helcopters n GPS-dened envronments, Journal of Feld Robotcs, vol. 28, no. 6, pp , 211. [Onlne]. Avalable: [12] B. Hérssé, T. Hamel, R. Mahony, and F.-X. Russotto, A terranfollowng control approach for a VTOL unmanned aeral vehcle usng average optcal flow, Autonomous Robots, vol. 29, pp , 21. [13] J. Zufferey and D. Floreano, Toward 3-gram autonomous ndoor arcraft: Vson-based obstacle avodance and alttude control, n Proceedngs of the IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 25. [14] V. Lppello, G. Loanno, and B. Sclano, Mav ndoor navgaton based on a closed-form soluton for absolute scale velocty estmaton usng optcal flow and nertal data, n Decson and Control and European Control Conference (CDC-ECC), 211 5th IEEE Conference on, dec. 211, pp [15] V. Grabe, H. Bulthoff, and P. Gordano, Robust optcal-flow based self-moton estmaton for a quadrotor uav, n Intellgent Robots and Systems (IROS), 212 IEEE/RSJ Internatonal Conference on, oct. 212, pp [16] S. Wess, M. W. Achtelk, S. Lynen, M. Chl, and R. Segwart, Realtme onboard vsual-nertal state estmaton and self-calbraton of MAVs n unknown envronments, n IEEE Internatonal Conference on Robotcs and Automaton (ICRA), 212. [17] Y. Ma, S. Soatto, J. Kosecka, and S. Sastry, An nvtaton to 3D vson : from mages to geometrc models, Sprnger, Ed. Sprnger, 2. [18] A. Martnell, State estmaton based on the concept of contnuous symmetry and observablty analyss: The case of calbraton, Robotcs, IEEE Transactons on, vol. 27, no. 2, pp , aprl 211. [19] J. Kelly and G. S. Sukhatme, Vsual-nertal sensor fuson: Localzaton, mappng and sensor-to-sensor self-calbraton, Internatonal Journal of Robotcs Research (IJRR), vol. 3, no. 1, pp ,