Trajectory Optimization for Self-Calibration and Navigation

Save this PDF as:

Size: px
Start display at page:

Download "Trajectory Optimization for Self-Calibration and Navigation"


1 Robotis: Siene and Sstems 7 Cambridge, MA, USA, Jul -6, 7 Trajetor Optimization for Self-Calibration and Navigation James A. Preiss, Karol Hausman, Gaurav S. Sukhatme Department of Computer Siene Universit of Southern California, Los Angeles, USA Stephan Weiss Institute of Smart Sstem Tehnologies Alpen-Adria-Universitat Klagenfurt, Klagenfurt, Austria Abstrat Trajetor generation approahes for mobile robots generall aim to optimize with respet to a ost funtion suh as energ, exeution time, or other mission-relevant parameters within the onstraints of vehile dnamis and obstales in the environment. We propose to add the ost of state observabilit to the trajetor optimization in order to ensure fast and aurate state estimation throughout the mission while still respeting the onstraints of vehile dnamis and the environment. Our approah finds a dnamiall feasible estimation-optimized trajetor in a sequene of onneted onvex poltopes representing free spae in the environment. In addition, we show a statistial proedure that enables observabilit-aware trajetor optimization for heterogeneous states in the sstem both in magnitude and units, whih was not supported in previous formulations. We validate our approah with extensive simulations of a visualinertial state estimator on an aerial platform as a speifi realization of our general method. We show that the optimized trajetories lead to more aurate navigation while eliminating the need for a separate alibration proedure. I. INTRODUCTION Suessful mobile robot ontrol requires aurate and robust state estimation. While state estimation algorithms have reeived signifiant attention from the robotis ommunit, it is also possible to improve state estimation from the planning perspetive. A arefull seleted trajetor based on information gain an provide better information to the exteroeptive sensors of the sstem. This so-alled ative pereption has been shown to notieabl redue failure modes of estimators in the past [3, 4, 5]. On the other hand, the aura of state estimation often depends not onl on the pereived exteroeptive information but also on the proprioeptive sstem input. For mobile robots these inputs are tpiall inertial readings of aeleration and angular veloit, and the are a diret result of the exeuted trajetor and the vehile dnamis. The proprioeptive sstem inputs are partiularl important for more omplex sensor fusion frameworks whih inlude self-alibration states and multiple sensors. To render high-dimensional state vetors observable, non-zero sstem inputs are generall required [6]. As a result, to improve the estimator s aura, trajetor planning should take the required sstem input for observabilit into aount. Hausman et al. [8] demonstrated an approah to generate trajetories that render speifi states better observable in a sstem performing online self-alibration. The method respets the sstem s dnamial onstraints and supports wapoint navigation problems, but assumes a free spae without Fig.. Minimum-snap and E LOG-optimized trajetories for a UAV navigation task. The E LOG trajetor optimizes our proposed observabilitbased ost funtion for a visual-inertial navigation sstem while remaining inside a orridor of onvex poltopes. The additional movement produes better observabilit of proprioeptive self-alibration states, leading to lower position estimate error in a simulated EKF at the end of the trajetor. obstales. In this paper, we remove the obstale-free requirement and provide a method that optimizes a trajetor for state observabilit while moving through a free spae orridor desribed b a sequene of onvex poltopes. We also desribe a saling proedure that aounts for the varing units, parameter value distributions, dnamis, and measurement model in a sstem with multiple self-alibration states. This proedure allows us to generate trajetories that balane the goals of onverging multiple self-alibration states. We demonstrate our approah using a visual-inertial state estimator on an aerial platform. The proposed approah, however, is not speifi to a partiular sstem or estimator tpe but optimizes the trajetories b analzing the nonlinear sstem dnamis and measurement model. The ke ontributions of our approah are: An optimization framework inluding a multi-state balaning strateg to optimize trajetories for estimation of high-dimensional state vetors. A suitable formulation of the trajetor optimization in obstale-free orridors using Bézier urves. Expliit appliation of the above general approah to a visual-inertial sstem on an aerial platform.

2 We evaluate the proposed method in extensive simulation experiments. We show that multi-state optimized trajetories effetivel balane the self-alibration goals of the individual states, ahieving ompetitive results to trajetories optimized for eah state in isolation. We demonstrate that an optimized trajetor from our framework outperforms ommon heuristi self-alibration trajetories and a state-of-the-art optimization method. Our results on the navigation task ield better position error than a ommon energ-effiient trajetor, showing the benefits of optimizing for navigation and self-alibration simultaneousl using our approah. II. RELATED WORK Past work on planning for state estimation an roughl be divided into two groups: exteroeptive, or environmentbased, and proprioeptive, or movement-based. Exteroeptive methods fous on analzing the environment around a robot and biasing the motion planning towards the most informative areas, generall b maximizing an information theoreti metri [3, 3, 5]. More reent work onsidered dense photometri image information b seeking highl textured surfaes [4]. Proprioeptive methods, whih inlude the method shown in this paper, fous on the how the robot should move to obtain the most aurate state estimates regardless of environment. Muh work in this area selets a speifi realization of a state estimator and minimizes the final state unertaint. With simple sstems it ma be possible to obtain an analti solution [], but more ommonl on omplex sstems a samplingbased approah with simulation is used [, ]. However, simulating the state estimator exposes the trajetor optimization method to an shortomings of the state estimator, partiularl with regard to linearization inonsisten as demonstrated in []. These methods also inherit the potentiall large omputational ost of the state estimator. In [8] the authors propose a ontinuous measure of observabilit from the non-linear observabilit analsis suggested in [9]. This method analzes the sstem dnamis and sensor model diretl and is not speifi to an partiular state estimator. For states diretl appearing in the sensor model, [] made use of this measure of observabilit to generate observabilit aware trajetories. The approah in [8] extends these methods to the qualit of observabilit of hidden states, i.e. states that do not appear in the measurement model. However, the method in [8] onl optimizes trajetories for estimation of a single self-alibration state at a time, and does not handle environmental obstales. In this paper, we remove those two limitations b introduing a multi-state saling tehnique and a trajetor representation based on Bézier urves that allows optimization with guaranteed obstale avoidane. In [], the authors present an alternative approah to observabilit analsis based on the volume of the set of indistinguishable trajetories for a given input sequene. While their approah has some ompelling advantages, suh as aounting for unknown noise inputs, it requires in-depth manual derivations for eah sstem analzed, and is not amenable to analzing subsets of self-alibration states. In ontrast, our method provides a reipe that produes a ost funtion in an user-hosen states from the sstem dnamis and measurement equations. III. PROBLEM FORMULATION We assume a nonlinear sstem of the following form: ẋ = f(x, u, δ), z = h(x, ɛ), where x is the state, u are the ontrol inputs, z are the outputs (sensor readings) and δ, ɛ are noise values aused b modeling errors and non-perfet atuators. We summarize the states that have onstant dnamis and are independent of the sstem inputs and other state variables as self-alibration states x s. A. Nonlinear Observabilit Analsis The observabilit of a sstem is defined as the possibilit to ompute the initial sstem state given a sequene of inputs u(t) and measurements z(t). A sstem is globall observable if there exist no two points x (), x () in the state spae with the same input-output u(t)-z(t) maps for an ontrol inputs. A sstem is weakl loall observable if there is no point x () with the same input-output map in a neighborhood of x () for a speifi ontrol input [8]. One wa to determine whether or not a nonlinear sstem is weakl loall observable is based on the rank of the nonlinear observabilit matrix [9], onstruted using the Lie derivatives of the sensor model h(x): O(x, u) = [ L h L h L h... ] T, () where L h i is the i-th Lie derivative of the sensor model h(x), defined reursivel as: L h = h(x), L h i+ = t Lh i = Lh i f(x, u), () x and L h i = Lh i x. In nonlinear sstems, the nonlinear observabilit is onl a loal propert that depends on the state and the input. In this definition, the observabilit of a sstem is onsidered as a binar propert, whih makes it unsuitable for gradient-based optimization methods. IV. EXPANDED EMPIRICAL LOCAL OBSERVABILITY GRAMIAN (E LOG) In order to optimize trajetories for state estimation, we use the notion of qualit of observabilit [8, 8]. A state is onsidered poorl observable if it leads to a small hange in the output, even when extensivel perturbed [8]. On the other hand, if a state auses a signifiant hange in the output, even when marginall perturbed, this state is onsidered well observable. Well-observable states are eas to estimate even in the presene of high measurement noise [9]. Following the derivation presented in [8] and using the Talor expansion of the sensor model about a point t, one an show that the Jaobian of the sensor model h(x) with respet to the state x around the time t is: x h t (t) = n (t t ) i L h i (t). (3) i! i=

3 In addition to showing the effet of the states that diretl influene the measurement, Eq. 3 also reveals the effets of the varing ontrol inputs and the states that are not inluded in the sensor model. In order to model the interations between the influenes that different states an have on the output, we use the loal observabilit Gramian [8]. We introdue the following notation for brevit: K t (t) = x h t (t) = x h t (x(t), u(t)). (4) Following [8], we use the Talor expansion of the sensor model to approximate the loal observabilit Gramian: W o (T, t) T K t (t + t) T K t (t + t)dt, (5) where t is a fixed horizon, hosen empiriall, that enables us to see the effets of the sstem dnamis. We refer to this approximation of the loal observabilit Gramian as Expanded Empirial Loal Observabilit Gramian (E LOG). To measure the qualit of observabilit we use the smallest singular value of the E LOG W o (T, t). Intuitivel, the observabilit Gramian an be seen as a (ross-orrelated) measure of the sensitivit of the measurements with respet to state variations. Maximizing the smallest singular value leads to maximizing the observabilit of the least observable dimension of x s. In ontrast, maximizing the ondition number or trae of the E LOG are less appropriate. The ondition number aptures onl the ratio between the most observable and least observable subspaes. Similarl, maximizing the trae ma reward trajetories that render one state ver well observable while other states are unobservable. In ontrast to the empirial loal observabilit Gramian (ELOG) proposed in [8], the E LOG formulation is able to apture input-output dependenies that are not visible in the sensor model. This propert is ahieved b inorporating higher order Lie derivatives and evaluating the Talor expansion over a short time horizon. These states an onl be aptured in the ELOG if the trajetor is generated b integrating the sequene of ontrols u, but this results in numerial integration errors, espeiall for high-order sstems. We refer the reader to [8] for more details on E LOG. V. MULTI-STATE E LOG In general, entries in the K t matries Eq. 4 ma have widel different magnitudes. These magnitudes depend on man fators, inluding the phsial units, measurement model, sstem dnamis, and the expeted values of the self-alibration states. As mentioned in [8], saling of these states is needed to ensure that the E LOG smallest-singular-value metri balanes the influene of all states equall. We introdue a olumn saling in the form of K t = K t diag(s) (6) as eah olumn of K t reflets the sensitivit of the measurement funtion with respet to one state. The values of s are determined empiriall b the following proedure: Generate a set of n phsiall plausible random trajetories ranging from stationar to near the phsial limits of the dnami model. For eah trajetor, randoml sample m sets of realisti self-alibration parameter values. For eah trajetor-parameter pair, evaluate K at p points. Let s i be the standard deviation of all entries in the i th olumn of all n m p generated K matries. This proedure approximates a uniform sampling from the distribution of K matries for the given sstem. In Se. VIII, we demonstrate that for our example sstem, this saling proess in a optimization for all self-alibration states an produe trajetories that perform nearl as well as trajetories optimized for the individual states in isolation. This proedure aims to eliminate issues aused b different sales of the elements om the K t matries. In partiular, states that minimall ontribute to the magnitude of hange of the measurement ma be swamped b other states than have muh larger absolute values (e.g. position of the vehile in the world frame vs. the aelerometer bias). In our experiments, this problem aused our nonlinear optimization tools to fail at optimizing the trajetor l for multiple states aording to the E LOG objetive, returning results lose to the initial guess. B appling a saling fator to the olumns of the K t matrix, we retain important properties of K t suh as the ratio between different partial measurement derivatives w.r.t. different states, while improving the behavior of E LOG as an optimization objetive. It is suffiient to perform this proedure one for a given sstem setup and use the stored s vetor for different problem instanes. VI. POLYNOMIAL TRAJECTORY BASES FOR OPTIMIZATION In this setion, we introdue trajetor optimization methods for optimizing E LOG for differentiall flat sstems. The lass of differentiall flat sstems inludes man vehiles whih might require online self-alibration, suh as ars, tratortrailers, fixed-wing airraft, and quadrotor heliopters [, ]. However, we emphasize that the E LOG ost funtion itself is not restrited to differentiall flat sstems. For a differentiall flat sstem, there exists a set of flat outputs suh that, given a trajetor (t), the sstem states x(t) and ontrol inputs u(t) an be omputed as funtions of the flat outputs and a finite number of their derivatives: x = ζ(, ẏ, ÿ,..., (n) ), u = ψ(, ẏ, ÿ,..., (m) ). (7) This assumption allows us to plan trajetories in the spae of flat outputs that guarantee kinemati feasibilit as long as the trajetor is suffiientl smooth. The two trajetor representations desribed here both result in pieewise polnomial trajetories, but favor different tasks. For generating losed-loop self-alibration trajetories, we use a null-spae representation that redues the number of optimization variables. For planning trajetories through a map with obstales, we use a Bézier urve formulation, where we

4 an onstrain the trajetor to lie inside a orridor of pairwise interseting onvex poltopes using onl linear onstraints on the deision variables. A. Pieewise Polnomial Null-Spae Basis A d-degree, q-piee pieewise polnomial takes the form: p T t(t) if t t < t (t) =. (8) p T q t(t) if t q t t q, where p i R d+ is the vetor of polnomial oeffiients for the i th polnomial piee, and t is the time vetor, i.e.: t(t) = [ t t... t d] T. As detailed in [8, 3], wapoint and ontinuit onstraints on the trajetor an be represented as a linear sstem: A [ ] p T... p T T q Ap = b. (9) With a high enough degree d, this sstem is underdetermined. We an therefore represent an solution b the form: p = p + Null(A)ρ, () where p is an partiular solution of Ap = b, suh as the minimum-norm solution provided b the Moore-Penrose pseudoinverse. This onverts an optimization problem over the spae of wapoint- and ontinuit-satisfing pieewise polnomials from a onstrained, q(d + )-dimensional problem into a smaller, unonstrained problem over the null spae weights ρ. The one-dimensional formulation given here extends naturall to higher-dimensional outputs. Sine the E LOG objetive funtion does not have an easil omputed gradient with respet to the polnomial oeffiients p, we approximate the gradient b forward differenes; therefore reduing the number of variables speeds up optimization signifiantl. B. Bézier Basis In pratial robot deploments, it ma be useful to have the abilit to self-alibrate while performing some other task, rather than pausing to exeute a losed-loop alibration trajetor. For a mobile robot, this means the robot should optimize its trajetor for self-alibration while moving from a t position to a goal position and avoiding environmental obstales. However, planning with polnomial oeffiients as optimization variables is not well suited to problems with omplex onfiguration spae obstales, and this propert extends to the null-spae basis. In previous work on polnomial trajetories [4], the authors hek ollisions at a finite set of sampled points, and resolve them b adding wapoints from a known safe pieewise linear trajetor. However, this method ma fail to detet ollisions in between the sample points, and eah resolved ollision requires re-solving the optimization problem with more variables. Instead, we use a Bézier urve basis similar to [6, 6] that provides ollision avoidane guarantees. We assume that a map of onfiguration-spae obstales is available and that a high-level planner has identified a orridor of pairwise-overlapping onvex poltopes ontaining some kinematiall feasible path from t to goal position in the map. Suh a orridor an be found using, e.g., the method of [5]. We seek a trajetor from t to goal that minimizes our E LOG ost funtion while remaining inside this orridor. From a high-level planner, we obtain the t and goal positions p t, p goal R k, and a sequene of n onvex poltopes C: C = P,..., P n, P i = {x R k : A i x b i }, () where (A i R m k, b i R m ) is the half-spae representation of the poltope P i. Furthermore, we require that a path from p t to p goal exist in C: P i P i+, p t P, p goal P n. () Note that the requirement of overlap between adjaent P i is suffiient to ensure that a path exists beause we are working with a differentiall flat sstem. Also note that there is no limit on the amount of overlap between an pair P i, P j and that P i need not be bounded in general. We seek an n-piee polnomial trajetor suh that the i th polnomial piee is ontained in P i. Bézier urves provide a natural basis for expressing suh trajetories. A degree-d Bézier urve is defined b a sequene of d + ontrol points x i R k and a fixed set of Bernstein polnomials, suh that f(t) = b,d (t)x + b,d (t)x + + b d,d (t)x d (3) for t [, ], where eah b i,d is a degree-d polnomial with oeffiients given in [4]. This form ma be interpreted as a smooth interpolation between x and x d. The urve begins at x and ends at x d. In between, it does not pass through the intervening ontrol points, but rather is guaranteed to lie in their onvex hull. This follows diretl from the fat that, on the interval [, ], the Bernstein polnomials are nonnegative and form a partition of unit [4]; thus an point in the form of Eq. 3 is a onvex ombination of the ontrol points x i. Thus, when using ontrol points as deision variables instead of monomial oeffiients, onstraining the ontrol points to lie inside the poltope P i guarantees that the resulting urve will lie inside P i also. Poltope onstraints take the form: A i x j b i, j [... d + ] (4) for eah ontrol point x j in the polnomial piee orresponding to the i th poltope. Enforing arbitrar levels of ontinuit in pieewise Bézier urves is also eas. The derivative of f(t) as denoted in Eq. 3 is another Bézier urve of degree d, with ontrol points that are saled forward differenes of the ontrol points of f(t): f (t) = db,d (t)(x x ) + + db d,d (t)(x d x d ) (5) This is a linear transformation of the ontrol points. We ma appl this relationship reursivel to generate equalit

5 onstraints on the ontrol points of adjaent piees up to the desired level of smoothness. (Note that b h,d () = for h and b h,d () = for h d.) In omparison to the null-spae formulation, the Bézier basis is desirable beause it enfores a ollision-free path through the orridor using onl linear onstraints. However, the number of optimization variables is larger than in the null-spae formulation, and additional nonlinear onstraints are still needed to enfore dnami limits. Thus, optimization in the Bézier basis is somewhat slower than in the null-spae basis. It is also true that the Bézier basis is onservative: for a poltope P, there exist ontrol points x,..., x d suh that some x i / P but the Bézier urve through x,..., x d lies inside P. However, for the sstem in this paper, the onservatism of the Bézier basis does not prevent our method from finding trajetories that perform well in experiments. Both the null-spae and Bézier bases require that the user speif the time interval for eah polnomial piee. For the losed-loop self-alibration problem this is of little onern, but in the Bézier basis it introdues an undesirable oupling between the size of the poltopes P i and the speed of the vehile moving through those poltopes. This issue an be addressed b several means: ) alloating different durations to eah poltope aording to a size-based heuristi, ) subdividing large poltopes until all poltopes are roughl the same size, 3) growing the poltopes so their overlap is maximized, whih allows the optimizer more freedom to ontrol the relative sizes of the polnomial segments, or 4) inluding time alloations as additional optimization variables. Of these, 3) is preferable beause it relaxes the poltope-polnomial oupling without inreasing the size of the optimization problem. C. Numerial Optimization Method The E LOG objetive funtion is nononvex in both the null-spae and Bézier bases. We are therefore limited to loal optimization methods. We use the MATLAB implementation of Sequential Quadrati Programming (SQP), whih an enfore nonlinear onstraints suh as maximum motor thrust via barrier funtions. Empirial tests showed that SQP performs faster than interior-point methods on our example problems. Multi-t optimization an be used to obviate the onern of piking an unusuall bad initial guess. In losed-loop self-alibration problems using the null-spae basis, we generate initial guesses of ρ b randoml sampling from a normal distribution and disarding samples that violate the nonlinear phsial onstraints. However, for orridor problems in the Bézier basis, the initial guess is nontrivial. One ould solve a feasibilit linear program to satisf the ontinuit and poltope onstraints, but this solution is not guaranteed to satisf the nonlinear phsial onstraints, and often does not in pratie. Instead, we minimize an integrated-squaredderivative ost funtion using quadrati programming as in [6] and use the solution as an initial guess. The order and relative weights of the derivatives in this ost funtion should be hosen based on an analsis of the sstem dnamis as the relate to the differentiall flat variables. VII. VISUAL-INERTIAL STATE ESTIMATOR AND QUADROTOR As an example appliation of our method, we onsider a quadrotor heliopter equipped with an Inertial Measurement Unit (IMU) and a amera. The IMU ontains a grosope, ielding rotational veloit about the three bod axes, and an aelerometer, ielding the sum of aeleration and gravit in the same axes. The amera is input to a visual odometr algorithm (e.g. [7]) whih ields the sstem s 3D position in undefined sale, and sale-free attitude estimations with respet to its own visual frame. The visual-inertial sensor suite is popular and performs well in pratie. It also requires estimating a high-dimensional set of alibration parameters in different phsial units and sales, reating a hallenging task for trajetor optimization. The full sstem state onsists of the following: x = ( p i w, vw, i q i w, b ω, b a, λ, p i, q i, p w v, q w ) v, (6) where p i w, vw i and q i w are the position, veloit and orientation (represented as a quaternion) of the IMU in the world frame, b w and b a are the grosope and aelerometer biases, λ is the visual sale, and p i, q i are the relative position and orientation between the amera and the IMU in the IMU frame. q w v an be seen as the diretion of the gravit vetor in the visual map [6] whih drifts over time due to aumulated errors in the visual framework. p w v is the analogous visual drift in position. This setup is also known as a loosel oupled visual-inertial odometr approah and is desribed in more detail in [8]. The state is governed b differential equations desribed in [6, 7]. To aount for their temporal variations, the IMU biases are modeled as random proesses. In this setup, the self-alibration states x s are the grosope and aelerometer biases b ω, b a, the visual sale λ, pose of the amera sensor in the IMU frame p i, q i and the drift states pw v, q w v. Using the visual-inertial state vetor in Eq. 6, the sstem dnamis, and assuming the onnetion between the IMU and the amera is rigid, we define the visual sensor model following [8]: z pv = h p (x, n zpv ) = p w v + λc T (q w v )(pi w + C T (q i w )p i) + n zpv, z qv = h q (x, n zqv ) = q i q i w q w v + n zqv, where n zpv and n zqv are white Gaussian measurement noise variables and C (q) is the rotation matrix obtained from the quaternion q. The nonlinear observabilit analsis in [6] and [9] shows that the sstem is observable up to the global position and heading onl with appropriate inputs. The nonlinear observabilit matrix of this sstem has maximal rank after inluding the 4 th Lie derivative, hene, this is the order of the Talor expansion we use for generating the E LOG in our experiments. We selet a quadrotor heliopter as the example robot to arr the visual-inertial sstem. Visual-inertial sstems are espeiall popular on quadrotors due to their light weight and low power requirements. As shown b Mellinger and Kumar [], the quadrotor dnamis are differentiall flat in

6 ] final rmse [m/s b a b a onl final rmse [ratio] -3 λ λ onl final rmse [m] p i onl p i final rmse [deg] q i onl q i final rmse [deg].5.5 q v w q v w onl b a -onl ost (-σ min ) λ-onl ost (-σ min ) p i -onl ost (-σmin ) q i -onl ost (-σmin ) - -5 q v w -onl ost (-σmin ) Fig.. Comparison between trajetories l optimized for all self-alibration states (blue triangles), trajetories separatel optimized for individual self-alibration states (green s), and randoml generated trajetories that exite the sstem near its phsial limits (purple irles). x-axes represent E LOG ost for individual state onl. -axes represent EKF estimation error of individual state at trajetor termination. Shaded ellipses indiate one-σ prinipal omponent boundaries of eah trajetor lass. the flat outputs of x,, z position and aw θ. The remaining extrinsi states, i.e. roll and pith angles, are funtions of these outputs and their derivatives. To ensure phsiall plausible trajetories we plae inequalit onstraints on thrust-to-weight ratio (.5) and angular veloit ( π rad s ). These values are well within the limits of a small quadrotor. VIII. EXPERIMENTAL SIMULATION RESULTS We evaluate the proposed method on a simulated quadrotor heliopter equipped with the visual-inertial sstem desribed in Se. VII. We demonstrate the sstem on two tasks: a losedloop self-alibration trajetor using the null-spae basis and a orridor navigation trajetor using the Bézier basis. In both ases, we represent trajetories as degree-7 polnomials with C 4 ontinuit and require that all derivatives be zero at the beginning and end points. The integration step and the time horizon ( t) for E LOG are.s. To judge the effetiveness of a trajetor for self-alibration, we implement a state estimator for the visual-inertial sstem, simulate the sstem and estimator over the duration of the trajetor, and finall measure the aura of the estimator s belief relative to known ground truth values. As a realization of the state estimator, we emplo the popular Extended Kalman Filter (EKF). In partiular, we use the indiret formulation of an EKF [9] where the state predition is driven b IMU measurements. We hoose this state estimator due its abilit to work with various sensor suites and proven robustness in the quadrotor senario. In order to fairl evaluate trajetories aross a distribution of plausible values for self-alibration states, our experiments simulate the EKF multiple times per trajetor with randoml sampled ground truth self-alibration state values and randoml sampled initial estimation errors in the EKF state. The distributions from whih we sample are listed in Table I. Aggregating results over multiple random simulations redues the effet of piking an unusuall favorable or unfavorable set of ground-truth states. All input and measurement noise are zero-mean Gaussian with the following standard deviations: σ = deg/s in the grosope, σ = m/se in the aelerometer, σ =. m in the visual position, and ±. deg in the visual attitude. param. distribution error dist. unit desription b ω N (,.86) N (,.9) deg/s grosope bias b a N (,.) N (,.) m/s aelerometer bias λ N (,.) N (,.5) ratio visual sale p i N (,.) N (,.) meter vision-imu position qi (S) 3 3 deg vision-imu attitude qv w (S) 3 deg vision-world attitude TABLE I. Distributions of randoml sampled self-alibration parameters and initial self-alibration estimate errors for simulation experiments. A. Self-Calibration For the self-alibration task, we optimize a 6-piee, 5- seond polnomial in the null-spae basis, ting and ending at the origin. The trajetor is onstrained to lie within a meter box and subjet to the thrust and angular veloit onstraints. The box onstraint is small enough to easil fit in a tpial indoor environment. As a baseline for omparison, we generate ompetitive random trajetories b randoml sampling sets of large nullspae weights and disarding trajetories that violate the phsial limits. The analsis in [] suggests that trajetories with large angular veloities lead to well observable selfalibration states. Eah random trajetor is then used as the initial guess for the SQP-based optimization proedure desribed in Se. VI, ielding a set of optimized trajetories. ) Validation of multi-state E LOG saling: To validate our proposed multi-state E LOG saling proedure, we ompare a set of trajetories l optimized for all self-alibration states against trajetories individuall optimized for the submatrix of a single self-alibration state. Results are shown in Fig.. Note that we do not optimize for the state p w v as the global position of this sstem is unobservable [6]. In eah satter plot, the x axis orresponds to the E LOG ost funtion for the individual alibration state, and the axis orresponds to the error of the individual state estimate in the EKF at the end of the trajetor. Final error for eah trajetor is averaged over five sets of randoml sampled ground truth self-alibration parameters. We see that, while the l optimized trajetories do not sore as highl on the individual-state E LOG ost funtions, the perform equall well or nearl as well in the EKF error metri. This suggests that the multi-state E LOG suessfull balanes the goals of optimizing eah

7 E LOG trae om figure x z z x z x x z Fig. 3. Trajetories used for the omparison in Fig. 4. Eah trajetor lasts 5 seonds and is tight against at some phsial or box onstraint. final error [m/s ] b a final error [ratio].5..5 λ final error [m] p i final error [deg] q i final error [deg] q v w ] int error [m/s E LOG trae b a E LOG trae int error [ratio] E LOG trae E LOG trae λ int error [m] E LOG trae E LOG trae p i int error [deg] E LOG trae Fig. 4. Quartile box plots summarizing final (top) and integrated (bottom) error of EKF state estimates, aggregated over 3 simulated runs. E LOG: trajetor l optimized for all self-alibration states using our framework. trae: trajetor minimizing final trae of EKF ovariane in all self-alibration states. : random trajetories near the sstem phsial limits., : ommon manuall designed heuristi self-alibration trajetories. E LOG trae q i int error [deg] E LOG trae E LOG trae q v w self-alibration state. Note that, in initial experiments without olumn saling, SQP frequentl terminated at a solution near the initial guess, indiating that the unsaled ost funtion is poorl onditioned with respet to the optimization variables. Both the individual and optimized trajetories signifiantl exeed the performane of random trajetories on the b a, p i, and qw v parameters. On qi, all three lasses perform roughl equall, but we note that the tpial estimation error of. degrees is ver low and an be onsidered suessfull onverged in all ases. On λ, the optimized trajetories perform equall well as the random trajetories, but here the estimation error of.3% is also quite small. ) Comparison to EKF-trae-minimization and heuristis: To explore the harateristis of these trajetories in greater detail, we randoml pik one representative -optimized trajetor and one random trajetor, and ompare them to several ompetitive baselines, with results over man simulated EKF runs with randomized self-alibration parameters (N = 3) displaed as box plots. As a baseline for omparison, we generate one trajetor that minimizes the integrated trae of the ovariane matrix in a simulated EKF [7]. This trajetor is optimized using the null-spae formulation with the same parameters and onstraints used for the E LOG-optimized trajetories. (Note that it is not feasible to generate man EKF-optimized trajetories in the manner of Fig. beause optimizing for the EKF trae took over 5 longer than optimizing for E LOG.) We also ompare against the ommon heuristi self-alibration trajetories and figure-8. These trajetories are designed manuall and spatiall saled so the are just within the same phsial onstraints used in the optimization proedure. All trajetories are visualized in Fig. 3. Results are shown in Fig. 4. The multi-state E LOGoptimized trajetor performs best in both integrated and final error in all self-alibration parameters, exept for final error in b a where the EKF-trae optimized trajetor is slightl better. Note that eah boxplot represents one trajetor, so larger interquartile range indiates that the aura of state estimate when exeuting that trajetor varies widel depending on the ground truth values and initialization. These results indiate that the E LOG-optimized losed-loop trajetor renders the self-alibration states more well observable than other trajetories. An espeiall interesting result is the trajetories optimized

8 z [m] rmse position [m] min-snap E LOG ±σ ±σ [m] - x [m] Fig. 5. Self-alibration trajetor optimized for estimation of the visual sale parameter λ. The trajetor full exploits, but does not violate, the box onstraints to provide best input (aeleration in this ase) to the sstem. for the visual sale parameter λ. An example is shown in Fig. 5. The optimizer exploits the entire optimization spae to provide best information linear aeleration input in the this ase while respeting the box and dnami onstraints. B. Planning in a orridor We demonstrate the orridor planning appliation in a manuall-designed orridor of moderate omplexit. As a baseline, we use a minimum-snap trajetor, omputed using quadrati programming as in [6]. Minimum-snap trajetor planning is widel used for quadrotors [, 4]. However, the energ-minimizing harateristi that makes these trajetories desirable for graeful flight or aggressive maneuvers an also lead to trajetories that do not exite the sstem suffiientl to render the self-alibration states well observable. We ompare this to a trajetor from our framework, optimized for the multi-state E LOG of all self-alibration states. The orridor and trajetories are visualized in Fig. at the beginning of this paper. The min-snap trajetor is harateristiall smooth. In ontrast, the E LOG-optimized trajetor displas signifiant additional movement to exite the sstem while remaining safel inside the orridor. We ompare the two trajetories fitness for self-alibration b simulating the EKF multiple times (N = 3) for both trajetories with randoml sampled ground truth x s and initialization errors aording to Table I. To present the results onisel, we use the root-mean-square error of the EKF position estimate as a prox for the overall aura of the self-alibration estimate. This orresponds with the end purpose of self-alibration, whih is to improve the estimation qualit of the robot s fundamental onfiguration-spae states. Results are shown in Fig. 6. We plot the mean RMS error of the position estimate at eah timestep. The shaded bands indiate the standard deviation of RMS errors aross the trials at eah timestep. These plots show that the E LOG-optimized trajetor is generall able to improve its position estimate over time b orreting the initial self-alibration estimate errors in the EKF. In ontrast, the min-snap trajetor displas poor onvergene. The wider standard deviations indiate that the EKF is unable to orret the initialization errors and is thus highl sensitive to the orretness of the initial alibration time [se] Fig. 6. RMSE of position estimate in EKF averaged over 3 simulated trials for min-snap and E LOG-optimized trajetories shown in Fig.. Ground truth self-alibration states and EKF initialization errors are randoml sampled for eah trial. One-σ bands illustrate the variation in estimation aura over different trials. IX. CONCLUSION In this work, we introdued a framework to optimize robot trajetories for estimation of self-alibration states. Our E LOG ost funtion emplos a short-horizon Talor expansion to apture the interations between the self-alibration states, sstem dnamis, and measurements, allowing us to optimize for states that do not diretl appear in the measurement model. We also desribed a statistial saling tehnique to l optimize for multiple self-alibration states with varing units, ranges, and measurement funtions, allowing us to balane the goals of estimating eah of the five selfalibration states in a visual-inertial odometr sstem. We desribed two trajetor representations: a pieewise polnomial null-spae basis for optimized self-alibration loop trajetories, and a Bézier spline basis for alibration-aware navigation through a orridor of interseting onvex poltopes. We demonstrated our method in extensive simulation experiments with an EKF-based visual-inertial odometr sstem. Our experiments showed a strong orrelation between the E LOG ost funtion value and the estimation aura of selfalibration states in an EKF, and validated that our statistial saling tehnique produes trajetories that balane onversion of all the self-alibration states simultaneousl. We showed that a single -optimized trajetor from our framework outperforms a trajetor minimizing the EKF ovariane trae and ommon heuristi self-alibration trajetories, while taking signifiantl less time to optimize than the EKF-based ost funtion. For the orridor navigation problem, we showed example results from our method using the Bézier urve trajetor representation and analzed the overall performane of the trajetories b onsidering the error of the vehile position estimate. Our optimized trajetor finished with better RMS position error than a minimum-snap trajetor, showing that a alibration-aware trajetor an help a mobile robot maintain an aurate overall state estimate while moving ollision-free through the environment.

9 REFERENCES [] M.W. Ahtelik, S. Weiss, M. Chli, and R. Siegwart. Path planning for motion dependent state estimation on miro aerial vehiles. In Pro. of the IEEE Int. Conf. on Robotis & Automation (ICRA), 3. [] Rik Bähnemann, Mihael Burri, Enri Galeran, Roland Siegwart, and Juan Nieto. Sampling-based motion planning for ative multirotor sstem identifiation. arxiv preprint arxiv:6.543, 6. [3] A. Br and N. Ro. Rapidl-exploring random belief trees for motion planning under unertaint. In Pro. of the IEEE Int. Conf. on Robotis & Automation (ICRA),. [4] Gabriele Costante, Christian Forster, Jeffre Delmerio, Paolo Valigi, and Davide Saramuzza. Pereption-aware path planning. arxiv preprint arxiv:65.45, 6. [5] Robin Deits and Russ Tedrake. Computing large onvex regions of obstale-free spae through semidefinite programming. In Algorithmi Foundations of Robotis XI, pages 9 4. Springer, 5. [6] Melvin E Flores. Real-time trajetor generation for onstrained nonlinear dnamial sstems using non-uniform rational b-spline basis funtions. Ph.D. Dissertation, California Institute of Tehnolog, 8. [7] K. Hausman, J. Müller, A. Hariharan, N. Aanian, and G. Sukhatme. Cooperative multi-robot ontrol for target traking with onboard sensing. The International Journal of Robotis Researh, 34(3):66 677, 5. [8] Karol Hausman, James Preiss, Gaurav Sukhatme, and Stephan Weiss. Observabilit-aware trajetor optimization for self-alibration with appliation to uavs. arxiv preprint arxiv:64.795, 6. [9] R. Hermann and A. Krener. Nonlinear ontrollabilit and observabilit. IEEE Transations on automati ontrol, (5): 78 74, 977. [] J. Hernandez, K. Tsotsos, and S. Soatto. Observabilit, identifiabilit and sensitivit of vision-aided inertial navigation. In 5 IEEE International Conferene on Robotis and Automation (ICRA), pages 39 35, Ma 5. [] J. Hesh, D. Kottas, S. Bowman, and S. Roumeliotis. Cameraimu-based loalization: Observabilit analsis and onsisten improvement. The International Journal of Robotis Researh, 3. [] B.T. Hinson and K.A. Morgansen. Observabilit optimization for the nonholonomi integrator. In Amerian Control Conferene (ACC), 3, pages , June 3. [3] Vadim Indelman, Lua Carlone, and Frank Dellaert. Planning in the ontinuous domain: A generalized belief spae approah for autonomous navigation in unknown environments. The International Journal of Robotis Researh, 34(7):849 88, 5. [4] Kenneth I Jo. Bernstein polnomials. On-Line Geometri Modeling Notes, 3,. [5] B.J. Julian, M. Angermann, M. Shwager, and D. Rus. Distributed roboti sensor networks: An information-theoreti approah. Int. Journal of Robotis Researh, 3(),. [6] J. Kell and G. Sukhatme. Visual-inertial sensor fusion: Loalization, mapping and sensor-to-sensor self-alibration. The International Journal of Robotis Researh, 3(),. [7] Georg Klein and David Murra. Parallel traking and mapping for small ar workspaes. In Mixed and Augmented Realit, 7. ISMAR 7. 6th IEEE and ACM International Smposium on, pages IEEE, 7. [8] A. Krener and K. Ide. Measures of unobservabilit. In Deision and Control, 9 held l with the 9 8th Chinese Control Conferene. CDC/CCC 9. Proeedings of the 48th IEEE Conferene on, pages , 9. [9] S. Lnen, M. Ahtelik, S. Weiss, M. Chli, and R. Siegwart. A robust and modular multi-sensor fusion approah applied to mav navigation. In In IEEE/RSJ Int. Conf. on Intelligent Robots and Sstems (IROS), pages , 3. [] Phillipe Martin, Rihard M Murra, and Pierre Rouhon. Flat sstems, equivalene and trajetor generation. 3. [] A. Martinelli and R. Siegwart. Observabilit properties and optimal trajetories for on-line odometr self-alibration. In Deision and Control, 6 45th IEEE Conferene on, pages , De 6. [] D. Mellinger and V. Kumar. Minimum snap trajetor generation and ontrol for quadrotors. In In Int. Conf. on Robotis and Automation (ICRA), pages IEEE,. [3] J. Müller and G.S. Sukhatme. Risk-aware trajetor generation with appliation to safe quadrotor landing. In In Pro. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Sstems (IROS), Chiago, IL, USA, September 4. [4] C. Rihter, A. Br, and N. Ro. Polnomial trajetor planning for aggressive quadrotor flight in dense indoor environments. In In Pro. of the International Smposium on Robotis Researh (ISRR), 3. [5] D. N. Ta, M. Kobilarov, and F. Dellaert. A fator graph approah to estimation and model preditive ontrol on unmanned aerial vehiles. In 4 International Conferene on Unmanned Airraft Sstems (ICUAS), pages 8 88, Ma 4. [6] Sarah Tang and Vija Kumar. Safe and omplete trajetor generation for robot teams with higher-order dnamis. In IEEE/RSJ International Conferene on Intelligent Robots and Sstems (IROS), 6. [7] S. Weiss, M. Ahtelik, S. Lnen, M. Chli, and R. Siegwart. Real-time onboard visual-inertial state estimation and selfalibration of mavs in unknown environments. In IEEE Int. Conf. on Robotis and Automation (ICRA),. [8] S. Weiss, M.. Ahtelik, S. Lnen, M. Ahtelik, L. Kneip, M. Chli, and R. Siegwart. Monoular vision for long-term miro aerial vehile state estimation: A ompendium. Journal of Field Robotis, 3(5):83 83, 3. [9] Stephan Weiss. Vision Based Navigation for Miro Heliopters. Phd thesis, ETH Zurih, Marh.