Trajectory Optimization for SelfCalibration and Navigation


 Anissa Bailey
 3 months ago
 Views:
Transcription
1 Robotis: Siene and Sstems 7 Cambridge, MA, USA, Jul 6, 7 Trajetor Optimization for SelfCalibration 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 AlpenAdriaUniversitat 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 missionrelevant 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 estimationoptimized trajetor in a sequene of onneted onvex poltopes representing free spae in the environment. In addition, we show a statistial proedure that enables observabilitaware 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 soalled 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 selfalibration states and multiple sensors. To render highdimensional state vetors observable, nonzero 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 selfalibration. The method respets the sstem s dnamial onstraints and supports wapoint navigation problems, but assumes a free spae without Fig.. Minimumsnap and E LOGoptimized trajetories for a UAV navigation task. The E LOG trajetor optimizes our proposed observabilitbased ost funtion for a visualinertial navigation sstem while remaining inside a orridor of onvex poltopes. The additional movement produes better observabilit of proprioeptive selfalibration states, leading to lower position estimate error in a simulated EKF at the end of the trajetor. obstales. In this paper, we remove the obstalefree 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 selfalibration states. This proedure allows us to generate trajetories that balane the goals of onverging multiple selfalibration states. We demonstrate our approah using a visualinertial 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 multistate balaning strateg to optimize trajetories for estimation of highdimensional state vetors. A suitable formulation of the trajetor optimization in obstalefree orridors using Bézier urves. Expliit appliation of the above general approah to a visualinertial sstem on an aerial platform.
2 We evaluate the proposed method in extensive simulation experiments. We show that multistate optimized trajetories effetivel balane the selfalibration 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 selfalibration trajetories and a stateoftheart optimization method. Our results on the navigation task ield better position error than a ommon energeffiient trajetor, showing the benefits of optimizing for navigation and selfalibration 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 movementbased. 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 nonlinear 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 selfalibration state at a time, and does not handle environmental obstales. In this paper, we remove those two limitations b introduing a multistate 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 indepth manual derivations for eah sstem analzed, and is not amenable to analzing subsets of selfalibration states. In ontrast, our method provides a reipe that produes a ost funtion in an userhosen 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 nonperfet atuators. We summarize the states that have onstant dnamis and are independent of the sstem inputs and other state variables as selfalibration 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 inputoutput u(t)z(t) maps for an ontrol inputs. A sstem is weakl loall observable if there is no point x () with the same inputoutput 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 ith 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 gradientbased 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. Wellobservable 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 (rossorrelated) 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 inputoutput 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 highorder sstems. We refer the reader to [8] for more details on E LOG. V. MULTISTATE 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 selfalibration states. As mentioned in [8], saling of these states is needed to ensure that the E LOG smallestsingularvalue 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 selfalibration parameter values. For eah trajetorparameter 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 selfalibration 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 selfalibration, suh as ars, tratortrailers, fixedwing 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 losedloop selfalibration trajetories, we use a nullspae 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 NullSpae Basis A ddegree, qpiee 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 minimumnorm solution provided b the MoorePenrose pseudoinverse. This onverts an optimization problem over the spae of wapoint and ontinuitsatisfing pieewise polnomials from a onstrained, q(d + )dimensional problem into a smaller, unonstrained problem over the null spae weights ρ. The onedimensional formulation given here extends naturall to higherdimensional 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 selfalibrate while performing some other task, rather than pausing to exeute a losedloop alibration trajetor. For a mobile robot, this means the robot should optimize its trajetor for selfalibration 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 nullspae 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 resolving 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 onfigurationspae obstales is available and that a highlevel planner has identified a orridor of pairwiseoverlapping 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 highlevel 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 halfspae 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 npiee 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 degreed 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 degreed 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 nullspae formulation, the Bézier basis is desirable beause it enfores a ollisionfree path through the orridor using onl linear onstraints. However, the number of optimization variables is larger than in the nullspae 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 nullspae 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 nullspae and Bézier bases require that the user speif the time interval for eah polnomial piee. For the losedloop selfalibration 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 sizebased 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 poltopepolnomial oupling without inreasing the size of the optimization problem. C. Numerial Optimization Method The E LOG objetive funtion is nononvex in both the nullspae 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 interiorpoint methods on our example problems. Multit optimization an be used to obviate the onern of piking an unusuall bad initial guess. In losedloop selfalibration problems using the nullspae 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 integratedsquaredderivative 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. VISUALINERTIAL 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 salefree attitude estimations with respet to its own visual frame. The visualinertial sensor suite is popular and performs well in pratie. It also requires estimating a highdimensional 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 visualinertial 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 selfalibration 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 visualinertial 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 visualinertial sstem. Visualinertial 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 selfalibration states (blue triangles), trajetories separatel optimized for individual selfalibration states (green s), and randoml generated trajetories that exite the sstem near its phsial limits (purple irles). xaxes 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 thrusttoweight 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 visualinertial sstem desribed in Se. VII. We demonstrate the sstem on two tasks: a losedloop selfalibration trajetor using the nullspae basis and a orridor navigation trajetor using the Bézier basis. In both ases, we represent trajetories as degree7 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 selfalibration, we implement a state estimator for the visualinertial 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 selfalibration states, our experiments simulate the EKF multiple times per trajetor with randoml sampled ground truth selfalibration 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 groundtruth states. All input and measurement noise are zeromean 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 visionimu position qi (S) 3 3 deg visionimu attitude qv w (S) 3 deg visionworld attitude TABLE I. Distributions of randoml sampled selfalibration parameters and initial selfalibration estimate errors for simulation experiments. A. SelfCalibration For the selfalibration task, we optimize a 6piee, 5 seond polnomial in the nullspae 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 SQPbased optimization proedure desribed in Se. VI, ielding a set of optimized trajetories. ) Validation of multistate E LOG saling: To validate our proposed multistate E LOG saling proedure, we ompare a set of trajetories l optimized for all selfalibration states against trajetories individuall optimized for the submatrix of a single selfalibration 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 selfalibration parameters. We see that, while the l optimized trajetories do not sore as highl on the individualstate E LOG ost funtions, the perform equall well or nearl as well in the EKF error metri. This suggests that the multistate 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 selfalibration states using our framework. trae: trajetor minimizing final trae of EKF ovariane in all selfalibration states. : random trajetories near the sstem phsial limits., : ommon manuall designed heuristi selfalibration trajetories. E LOG trae q i int error [deg] E LOG trae E LOG trae q v w selfalibration 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 EKFtraeminimization 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 selfalibration 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 nullspae formulation with the same parameters and onstraints used for the E LOGoptimized trajetories. (Note that it is not feasible to generate man EKFoptimized 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 selfalibration trajetories and figure8. 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 multistate E LOGoptimized trajetor performs best in both integrated and final error in all selfalibration parameters, exept for final error in b a where the EKFtrae 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 LOGoptimized losedloop trajetor renders the selfalibration states more well observable than other trajetories. An espeiall interesting result is the trajetories optimized
8 z [m] rmse position [m] minsnap E LOG ±σ ±σ [m]  x [m] Fig. 5. Selfalibration 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 manualldesigned orridor of moderate omplexit. As a baseline, we use a minimumsnap trajetor, omputed using quadrati programming as in [6]. Minimumsnap trajetor planning is widel used for quadrotors [, 4]. However, the energminimizing 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 selfalibration states well observable. We ompare this to a trajetor from our framework, optimized for the multistate E LOG of all selfalibration states. The orridor and trajetories are visualized in Fig. at the beginning of this paper. The minsnap trajetor is harateristiall smooth. In ontrast, the E LOGoptimized trajetor displas signifiant additional movement to exite the sstem while remaining safel inside the orridor. We ompare the two trajetories fitness for selfalibration 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 rootmeansquare error of the EKF position estimate as a prox for the overall aura of the selfalibration estimate. This orresponds with the end purpose of selfalibration, whih is to improve the estimation qualit of the robot s fundamental onfigurationspae 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 LOGoptimized trajetor is generall able to improve its position estimate over time b orreting the initial selfalibration estimate errors in the EKF. In ontrast, the minsnap 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 minsnap and E LOGoptimized trajetories shown in Fig.. Ground truth selfalibration 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 selfalibration states. Our E LOG ost funtion emplos a shorthorizon Talor expansion to apture the interations between the selfalibration 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 selfalibration states with varing units, ranges, and measurement funtions, allowing us to balane the goals of estimating eah of the five selfalibration states in a visualinertial odometr sstem. We desribed two trajetor representations: a pieewise polnomial nullspae basis for optimized selfalibration loop trajetories, and a Bézier spline basis for alibrationaware navigation through a orridor of interseting onvex poltopes. We demonstrated our method in extensive simulation experiments with an EKFbased visualinertial 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 selfalibration states simultaneousl. We showed that a single optimized trajetor from our framework outperforms a trajetor minimizing the EKF ovariane trae and ommon heuristi selfalibration trajetories, while taking signifiantl less time to optimize than the EKFbased 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 minimumsnap trajetor, showing that a alibrationaware trajetor an help a mobile robot maintain an aurate overall state estimate while moving ollisionfree 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. Samplingbased motion planning for ative multirotor sstem identifiation. arxiv preprint arxiv:6.543, 6. [3] A. Br and N. Ro. Rapidlexploring 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. Pereptionaware path planning. arxiv preprint arxiv:65.45, 6. [5] Robin Deits and Russ Tedrake. Computing large onvex regions of obstalefree spae through semidefinite programming. In Algorithmi Foundations of Robotis XI, pages 9 4. Springer, 5. [6] Melvin E Flores. Realtime trajetor generation for onstrained nonlinear dnamial sstems using nonuniform rational bspline basis funtions. Ph.D. Dissertation, California Institute of Tehnolog, 8. [7] K. Hausman, J. Müller, A. Hariharan, N. Aanian, and G. Sukhatme. Cooperative multirobot 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. Observabilitaware trajetor optimization for selfalibration 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 visionaided 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. Cameraimubased 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. OnLine Geometri Modeling Notes, 3,. [5] B.J. Julian, M. Angermann, M. Shwager, and D. Rus. Distributed roboti sensor networks: An informationtheoreti approah. Int. Journal of Robotis Researh, 3(),. [6] J. Kell and G. Sukhatme. Visualinertial sensor fusion: Loalization, mapping and sensortosensor selfalibration. 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 multisensor 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 online odometr selfalibration. 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. Riskaware 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 higherorder 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. Realtime onboard visualinertial 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 longterm 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.