计算机视觉资料 SLM_VIO学习总结资料 Shojie Shen vinsmoil Shojie Shen vinsmoil Shojie Shen vinsmoil 1-23Tightly-coupled monoculr visul-inertil fusion for utonomous f

Tightly Coupled Monocular Visual Inertial Fusion for Autonomous Flight of Rotorcraft MAVs Shaojie Shen Nathan Michael and Vijay Kumar Abstract Therehavebeenincreasinginterestsinthe robotics community in building smaller and more agile au tonomous micro aerial vehicles MAVs In particular the monocular visual inertial system VINS that consists of only a camera and an inertial measurement unit IMU forms a great minimum sensor suite due to its superior size weight and power SWaP characteristics In this paper we present a tightly coupled nonlinear optimization based monocular VINS estimator for autonomous rotorcraft MAVs Our estimator allows the MAV to execute trajectories at 2 m s with roll and pitch angles up to 30 degrees We present extensive statistical analysis to verify the performance of our approach in different environments with varying fl ight speeds I INTRODUCTION Sensor equipped micro aerial vehicles MAVs with au tonomous fl ight capability are ideal platforms for missions in complex and confi ned environments due to its small size superior mobility and minimum operator workload While it is obvious that reducing the size of the MAV will make it more suitable for operations in confi ned environments it also poses tighter SWaP constrains As the size scales down a monocular visual inertial system VINS that consists of a camera and a low cost IMU becomes the only viable setup due to its ultra light weight and small footprint In fact monocular VINS is the minimum sensor suite that allows both autonomous fl ight and suffi cient environment awareness In this work we propose a tightly coupled optimization based monocular VINS estimator that enables autonomous fl ight of rotorcraft MAVs in unstructured and unknown environments This work builds on and is a substantial improvement from our recent work on linear initialization of monocular VINS 1 We identify the contribution of this work as threefold 1 The development of a tightly coupled nonlinear sliding window optimization framework for opti mal fusion of IMU and monocular camera measurements 2 The integration of initialization and marginalization schemes from our recent work into a complete monocular VINS estimator with analysis of choices of parameters and 3 Online experiments of autonomous fl ight through a variety of S Shen is with the Department of Electronic and Computer Engineering Hong Kong University of Science and Technology Hong Kong China eeshaojie ust hk N Michael is with the Robotics Institute Carnegie Mellon University Pittsburgh PA 15213 USA nmichael cmu edu V Kumar is with the GRASP Laboratory University of Pennsylvania Philadelphia PA 19104 USA kumar grasp upenn edu We gratefully acknowledge support from ARL grant W911NF 08 2 0004 ONR grants N00014 07 1 0829 and N00014 09 1 1051 and NSF grant IIP 1113830 Fig 1 Our quadrotor experimental platform equipped with an Intel NUC computer a MEMS IMU and a camera environments with statistical analysis of estimator and fl ight performance To the best of our knowledge we are the fi rst to demon strate autonomous fl ight with a monocular VINS sensor suite using a tightly coupled fusion approach We are able to achieve autonomous tracking of highly dynamical trajectories with a maximum velocity of 2m s and maximum roll pitch angles close to 30degrees Statistical analysis Sect VI suggests good performance with different speeds and in different environments Next we discuss literatures in similar fi elds in Sect II In Sect III we recap the basis of initialization of monocular VINS The derivation of the tightly coupled optimization framework is presented in Sect IV This is followed by a discuss of a two way marginalization scheme for handling degenerate motions that are unique to monocular VINS Sect V Sect VI fi rst discusses hardware setup and soft ware implementation details then presents multiple online experiments with performance analysis II RELATEDWORK Solutions to VINS with either monocular or stereo cam eras has been proposed using fi ltering frameworks 2 9 and with graph based optimization bundle adjustment frame works 10 12 Filtering based approaches may achieve faster processing due to its continuous marginalization of past states but early fi x of linearization points may results in sub optimal results Graph based approaches benefi t from iterative re linearization but they usually demand more com putational resources With proper marginalization a constant complexity sliding window graph based framework can be obtained 10 Conditioning is also a popular method among the computer vision community to achieve constant compu tation complexity 13 14 2015 IEEE International Conference on Robotics and Automation ICRA Washington State Convention Center Seattle Washington May 26 30 2015 978 1 4799 6923 4 15 31 00 2015 IEEE5303 We can also categorize VINS solutions as loosely cou pled 2 3 or tightly coupled 4 9 11 12 Loosely coupled approaches utilize independent vision processing modules such as PTAM 13 for up to scale pose estimation and integrate results from the vision module with IMU for scale estimation Tightly coupled approaches usually lead to better estimation results due to the direct and systematic fusion of feature and IMU measurements When it comes to VINS with only one camera there is a unique challenge in scale ambiguity due to degenerate mo tion It is well known that in order to render the scale observ able accelerations in at least two axes are required 5 6 15 However for a rotorcraft MAV degenerate motions such as hovering and constant velocity motions are unavoidable The hover case can be addressed via conditioning in a keyframe based loosely coupled approach 13 or with a last in fi rst out LIFO scheme in a tightly coupled sliding window approach 9 The monocular VINS sensor suite has been used for autonomous fl ight In 2 3 a monocular SLAM framework is used as the main vision processing pipeline In conjunction with a loosely coupled fi ltering framework these approaches successfully enable a quadrotor to fl y autonomously with a downward facing camera However due to the lack of direct scale measurement these approaches relies on the assumption of slowly varying or good initialization of the visual scale This can be diffi cult to enforce during fast motions at low altitudes with potentially rapid changes in the observed features III ON THE FLYINITIALIZATION A good initialization point is required for solving the highly nonlinear monocular VINS system In practice how ever this initialization point is usually hard to obtain due to the fact that the metric scale of the monocular VINS system is not directly observable In order to initialize the metric scale motions that consist of nonzero acceleration are required For a MAV this often results in unknown and nontrivial initial velocity and attitude gravity vector As such we require that the system is capable of on the fl y initialization to recover all critical states such as velocity gravity vector and depth of features The problem can be formulated as solving two sets of linear systems and it is discussed in our earlier work 1 Here we briefl y recap the rationale behind our approach We begin by defi ning notations We consider was the earth s inertial frame bas the current IMU body frame kas the camera frame while taking the kthimage Note that IMU usually runs at a higher rate than the camera and that multiple IMU measurements may exist in the interval k k 1 We assume that the camera and the IMU are pre calibrated such that the camera optical axis is aligned with the z axis of the IMU pX Y v X Y and R X Y are 3D position velocity and rotation of frame Y with respect to frame X In particular pX t represents the position of the body frame at time t with respect to frame X Similar conversion follows for other parameters gw 0 0 g Tis the gravity vector in the world frame and gkis the earth s gravity vector expressed in the body frame of the kthimage Given two time instants corresponding to two image frames the IMU propagation model for position and ve locity expressed in the world frame can be written as pw k 1 p w k vw k t ZZ t k k 1 Rw ta b t g w dt2 vw k 1 v w k Z t k k 1 Rw ta b t g w dt 1 where ab t is the linear acceleration in the body frame t is the time difference between k and k 1 It can be seen that the rotation between the world frame and the body frame is required in order to propagate the states with IMU measurements This rotation can only be determined if the initial attitude of the vehicle is known which is not the case during the initialization phase of monocular VINS However as suggested in 16 if the reference frame of the IMU propagation model is attached to the fi rst pose of the system i e the fi rst pose that we are trying to estimate 1 can be rewritten as p0 k 1 p 0 k R 0 kv k k t R 0 kg k t2 2 R0 k k k 1 vk 1 k 1 Rk 1 k vk k R k 1 k gk t Rk 1 k k k 1 gk 1 Rk 1 k gk 2 where k k 1 and k k 1 can be obtained solely with IMU measurements within k k 1 R0 k is the change in rotation since the fi rst pose or since the 0thimage and Rk k 1 is the incremental rotation between two images By decoupling optimization process of rotation and other quantities it is possible to recover all initial states in the monocular VINS system in a linear fashion More specifi cally rotation can be obtained by solving a linear system that incorporate short term integration of gyroscope measurements and relative epipolar constraints After the rotation is fi xed all other IMU states p0 k v k k g k as well as depth of features can also be solved linearly We refer readers to 1 for details of this on the fl y initialization process IV TIGHTLY COUPLEDNONLINEAROPTIMIZATION After the monocular VINS is initialized with the linear approach 1 we are able to use a tightly coupled nonlinear optimization framework to jointly optimize both the transla tion and rotation components of the system A Formulation We use a tightly coupled sliding window graph based formulation for nonlinear optimization to achieve both high accuracy and maintain bounded computation complexity The full state vector is defi ned as the transpose is ignored for the simplicity of presentation X x0 n x 0 n 1 x 0 n N 0 m 1 m M x0 k p0 k v k k q 0 k p0 0 0 0 0 q0 0 0 0 0 1 5304 where x0 k is the kthcamera state that consists of the pose with respect to the fi rst camera pose as well as the body frame velocity We use quaternions q qx qy qz qw to represent rotation in order to avoid singularities The Hamil ton notation is used for quaternions Note the dimension of the state vector is not the same as the dimension of the degree of freedom of the system The three dimensional ro tation is over parameterized by the four dimensional quater nion N is the number of camera states in the sliding window M is the number of all features that have suffi cient parallax within the sliding window n and m are starting indexes of states in the sliding window lis the depth of the lthpoint feature from its fi rst observation We aim to fi nd a confi guration of the state parameters that produce the maximum a posteriori estimate by minimizing the sum of the Mahalanobis norm of all measurement errors min X bp pX X k D rD zk k 1 X 2 Pk k 1 X l j C r C z j l X 2 Pj l 3 where pis the prior D and C are indexes of the set of IMU and camera measurements with the corresponding residuals defi ned as rD zk k 1 X and rC z j l X These quantities will be will be derived in Sect IV B and Sect IV C respectively Although the residuals for position velocity and feature depth can be easily defi ned p p p v v v 4 the residual for rotation is more involved Similar to 11 we use the perturbation of the tangent space of the rotation manifold as the minimum dimensional representation of the rotation residual The error quaternion term q is defi ned as the small difference between the estimated and the true quaternions q q q q 1 2 1 5 where is the quaternion multiplication operator From this we can use the three dimensional error vector as the representation of rotation residual Similarly we can write the error term in the form of rotation matrix R R I b c 6 where b c is the skew symmetric matrix from Following this defi nition we operate on the error state representation during the optimization X x0 n x 0 n 1 x 0 n N m m 1 m M x0 k p0 k v k k 0 k We linearize the cost function 3 with respect to X and iteratively minimize the cost of the resulting linear system Given the current best state estimates X we have min X bp p X X k D r D zkk 1 X Hk k 1 X 2 Pk k 1 X l j C r C z j l X Hj l X 2 Pj l 7 where Hk k 1 and Hj l which will also be defi ned in Sect IV B and Sect IV C are the Jacobians of rD zk k 1 X and rC zj l X with respect to X respectively The system 7 can be rewritten and solved as p D C X bp bD bC 8 after which the state estimates can be updated as X X X 9 where is the compound operator that has the form of simple addition for position velocity and feature depth as in 4 but is formulated as quaternion multiplication for rotations as in 5 B IMU Measurement Model We now present the formulation for the IMU measurement zk k 1 the measurement covariance matrix P k k 1 the residual rD zk k 1 X and the measurement Jacobian H k k 1 It should be fi rst noted that since there are multiple accelerometer and gyroscope measurements between two images the IMU measurement zk k 1 is a composition of multiple IMU readings zk k 1 k k 1 k k 1 qk k 1 RR t k k 1 Rk t abtdt2 R t k k 1 Rk t abtdt R t k k 1 b t qktdt 10 where ab t ab t ant b t b t nt are accelerometer and gyroscope measurements that are corrupted with additive noise and b t 1 2 b b t c b t b tT 0 Rk t can be derived from qk t Again while error terms for k k 1 and k k 1 are still additive since qk k 1 is over parameterized we defi ne its error terms as the perturbation from the true value qk k 1 q k k 1 1 2 k k 1 1 11 With the approximated rotation matrix composition of the error term 6 we can derive the continuous time linearized 5305 dynamics of the error terms from 10 and 11 0I0 00 Rk tb abt c 00 b b t c k t k t k t 00 Rk t 0 0 I an t nt Ft zk t Gtnt from which we can derived the fi rst order discrete time covariance update equation in order to recursively compute Pk k 1 with the initial covariance Pk k 0 Pk t t I Ft t P k t I Ft t T Gt t Qt Gt t T 12 where t k k 1 and t is the time between two IMU measurements and Qtis the covariance matrix for IMU measurements Following 2 and 11 we can now defi ne the measure ment residual rD zk k 1 X h k k 1 k k 1 k k 1 i T as k k 1 k k 1 k k 1 Rk 0 p0 k 1 p 0 k g 0 t2 2 vk k t k k 1 Rk 0 R0 k 1v k 1 k 1 g 0 t vk k k k 1 2 h qk 1 k 1 q 0 1 k q0 k 1 i xyz 13 where the gravity vector of the fi rst camera state g0is solved by the linear initialization 1 and xyzextracts the vector part of a quaternion Using 6 and the fact that R0 k 1 R0 k R k k 1 we can obtain the following by ignoring higher order terms 0 k 1 Rk 1 0 R0 k 0 k k k 1 which provides a simple linearized form of the propagation of rotation error terms As such the Jacobian of the IMU measurement residual with respect to the error state can be obtained as Hk k 1 rD xk rD xk 1 rD xk Rk 0 tIbRk 0 p0k 1 p0k g0 t 2 2 c 0 IbRk 0 R0k 1v k 1 k 1 g 0 t c 00 Rk 1 0 R0 k rD xk 1 Rk 0 00 0Rk 0 R0k 1 Rk 0 R0k 1bv k 1 k 1 c 00I 14 Equations 10 12 13 and 14 defi ne all required quantities to specify the IMU measurement model C Camera Measurement Model The formulation of the camera measurement model is straightforward The feature measurement is the observa tion of the feature in the normalized image plane zj l h uj l v j l i T The residual term is the reprojection error which is defi ned as rC zj l X fxj l fzj l uj l fyj l fzj l vj l fj l fxj l fyj l fzj l Rj 0 p0 i p0 j lR 0 iu i l 15 where ui l ui l v i l 1 T is the fi rst observation of the feature and it is considered as noiseless The residual covariance is the feature measurement noise matrix Pj l The Jacobian can be obtained by utilizing 6 and applying the chain rule on 15 Hj l 1 fzj l 0 fxj l fzj2 l 0 1 fzj l fyj l fzj2 l fj l xi fj l xj fj l l fj l xi h Rj 0 0Rj 0 R 0 i luil i fj l xj h Rj 0 0 j Rj 0 p0 i p0 j lR 0 iu i l ki fj l l Rj 0 R 0 iu i l 16 Equations 15 and 16 defi ne all required quantities to specify the camera measurement model V HANDLINGSCALEAMBIGUITY VIATWO WAY MARGINALIZATION For MAV applications due to limited onboard computa tional resources and the requirement of real time processing for feedback control we have to bound the complexity of the estimator by selectively marginalizing out camera states and features from the sliding window However due to the well known acceleration excitation requirement 5 6 15 for scale observability for monocular VINS a naive strategy that always marginalize the oldest state may result in unobserv able scale in degenerate motions such as hovering or constant velocity motions For hovering as proved in 9 if the vehicle fi rst under goes generic motions with suffi cient acceleration excitation then enters hovering the scale observability can be preserved by using a last in fi rst out LIFO sliding window scheme 9 performs state only measurement update during hovering and covariance is updated only once as the vehicle exits hovering However this approach will lead to pessimistic covariance as during hovering the covariance can grow arbitrary big while the actual estimation error is bounded For constant velocity motions the scale is unobservable as old states that correspond to generic motions will eventually be removed from the sliding window due to computation constraints However we can still perform scale propagation by marginalizing old states and correct scale drifting when the platform resumes generic motions at a later time Based on this discussion we propose to use a two way marginalization scheme to bound the computation cost and 5306 0 1 3 3 4 5 0 1 0 1 3 3 4 6 0 0 1 3 3 4 5 6 0 1 Fix States Float States a 0 1 3 3 4 5 0 1 0 1 3 3 4 5 0 1 6 1 3 3 4 5 1 6