IROS2019国际学术会议论文集 1690

Observability Analysis of Position Estimation for Quadrotors With Modifi ed Dynamics and Range Measurements Eranga Fernando Oscar De Silva George K I Mann and Raymond G Gosine Abstract This study performs a nonlinear observability analysis on range assisted inertial navigation system INS for quadrotor micro aerial vehicles MAV The INS is formulated incorporating the quadrotor dynamics with aerodynamic drag forces The observability analysis is carried out for cases where three and two range measurements are available The analysis facilitates the range assisted localization of MAVs when there are less than four range measurements are available The primary objective of this study is to identify the condi tions under which the INS becomes unobservable and these conditions are validated through numerical simulation The main contributions of this paper are as follows 1 Nonlinear observability analysis of the range assisted INS for quadrotor MAVs 2 Theoretical derivation and numerical validation of unobservable conditions for three and two range cases 3 Experimental validations of estimator performance I INTRODUCTION The objective of this study is to investigate an alternative approach for estimating the 3D position of quadrotor micro aerial vehicles MAV in indoor environments using RF range measurements to a set of beacons at known loca tions 3D position estimation using RF range measurements are less computationally expensive than other popular self localization methods such as visual simultaneous localiza tion and mapping SLAM visual inertial navigation and laser range fi nder assisted localization methods Additionally when compared with typical self localization sensors the performance of RF ranging does not depend on lighting conditions or the number of traceable features in the en vironment As a result RF methods can be robustly utilized in many workspaces where a set of ranging beacons can be pre deployed to support localization 1 Ultra wideband UWB technology has recently become one of the leading cost effective means of RF ranging with suffi cient accuracy to support robotic localization applications 2 3 Range based localization techniques can be divided into two main categories range only localization and range as sisted localization Range only localization is widely used in applications such as wireless sensor networks where multiple range measurements are available to estimate the position 4 Range assisted localization employs other aiding sensors as additional measurements for the state estimation framework 5 In theory it is possible to calculate the unique 3D position of a platform using a minimum of four range measurements with respect to a know constellation 6 Many range only Eranga Fernando O De Silva G K Mann and R G Gosine are with Faculty of Engineering and Applied Sciences Memorial University of Newfoundland St John s NL Canada A1B 3X5 email hctef2 oscar desilva gmann rgosine mun ca and range assisted quadrotor localization studies have as sumed that there are four or more range measurements avail able for localization 3 5 However there can be instances where a quadrotor has less number of range measurements during the fl ight due to coverage limitations measurement drops or sensor failures Furthermore in industrial appli cations such as infrastructures inspection it is desirable to have fewer number of range beacons to cover the working environment which in turn can reduce the overall deployment cost of the system compared to a typical system setup In order to facilitate fewer ranging sensors in a range assisted localization system one should fi rst identify the lim itations of the system To this end authors of 7 have shown that it is theoretically possible to calculate the 3D relative pose between two agents with only one range measurement between the platforms However the system considered in 7 assumes aiding sensors providing platform velocities which are not generally available in quadrotor MAVs Quadrotor localization can be effectively assisted using IMU measure ments similar to fi lters proposed in 3 Furthermore the study in 8 have shown the improved pose and velocity estimation accuracy achieved for MAVs by incorporating the aerodynamic drag forces in their estimation framework Hence for range assisted localization of quadrotor MAVs it is required to analyze the observability while considering IMU measurements and aerodynamic drag forces To the best of the authors knowledge this has not been addressed in the literature This paper performs a nonlinear observability analysis on the range assisted localization problem considering IMU measurements and using the quadrotor dynamic model with aerodynamic drag forces as part of the estimation model The scope of this paper is limited to the cases where there are three or two ranges available for range assisted localization The main contributions of this paper are as follows First this paper performs a nonlinear observability analysis of the range assisted INS for quadrotor MAVs for cases where less than four range measurements are available For this purpose the paper uses a dynamic model of the quadrotor MAV which captures the aerodynamic drag forces Second this paper identifi es and numerically validates the exact conditions under which the system loses observability Third this paper evaluates the performance of the estimator using experimental data obtained with a research grade MAV equipped with UWB ranging sensors 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE2783 II RELATEDWORK Authors in 7 have performed an observability analysis of 3D relative localization problem between two agents The analysis proves that 3D relative localization system with one range measurement is locally weakly observable when both platforms velocities are available and both platforms are non stationary Authors in 9 have conducted a similar study for relative localization problem with range and bearing mea surements between the platforms and establishes additional conditions on platform velocities for system observability Several studies have conducted observability analysis on estimating the position of autonomous underwater vehicles AUV using single range measurement In 10 authors have incorporated a depth sensor along with the range mea surement and conducted the observability analysis Authors in 11 have conducted a similar observability analysis for 3D position estimation of a generic platform with a single range measurement and have proposed a fi lter design to improve the robustness of the estimation The observability studies conducted in 7 9 11 have assumed that platform velocities can be measured and they do not consider any dynamic constraints relevant to the respective platform A fl ying quadrotor experiences several types of aerody namic drag forces Among the several types induced drag and blade fl apping drag are the most signifi cant 12 Recent studies have incorporated drag forces into the dynamic model for precise control of quadrotor MAVs in high speed maneuvers 13 14 Work in 8 uses the dynamic model with drag forces in their estimator formulation and have shown improved estimation accuracy compared to generic INS In the range assisted INS proposed in 3 authors have used the quadrotor dynamics with aerodynamic forces along with UWB range measurements to estimate the pose of the quadrotor However authors of 3 do not study the effect of the number of range measurements on the performance of the estimator In our previous work 15 a numerical analysis on range and height assisted pose estimation of quadrotor MAV is presented Performance of the estimator was analyzed for different combinations of range and height measurements However the numerical analysis did not provide insight into the conditions under which the system loses observability Since it is diffi cult to obtain accurate and reliable height measurements in unstructured environments the current study focuses only on range measurements and conducts a nonlinear observability analysis of the range assisted INS Then the observability conditions are derived for cases where three and two range measurements are available III PROBLEMFORMULATION This section presents the mathematical model of quadrotor dynamics incorporating the aerodynamic drag forces IMU and range measurements Throughout the paper the case with three range measurements is referred to as scenario S1 and the case with two range measurements is referred to as scenario S2 ZE XE YE ZB YB XB mg fT fd p C E B Fig 1 Inertial frame and quadrotor frame A System Dynamics In order to properly defi ne the problem framework let E denote the world frame and B denote the body frame of the quadrotor Orientation of the world frame and the body frame is shown if Figure 1 Position of the quadrotor body frame B expressed in E is denoted as p and the velocity of the quadrotor expressed in B is denoted by vb Hence the velocity of the quadrotor expressed in E can be defi ned as p Cvbwhere C is the rotation matrix from B to E In this study the orientation of the quadrotor is param eterized using unit quaternion q and the non Hamiltonian convention is used to defi ne the quaternions 16 q qxqyqzqw T q T qw T qTq 1 1 The rotation matrix C is defi ned as C q2 w q T q I3 2qw q 2 q qT 2 I3denotes the 3 3 identity matrix and q is the skew symmetric matrix of q Rotation kinematics can be expressed as q 1 2 0 q 3 where x y z Tis the angular rate of the quadrotor in B and denotes the quaternion multiplication Equa tion 3 can be expressed in matrix form as q R q T0 T 4 where R q q q qwI3 q q T 5 Quadrotor MAV dynamics are modeled as a 6 DOF rigid body with aerodynamic drag forces Hence the acceleration of the quadrotor can be expressed as p C fd fTe3 g 6 where g is the gravitational acceleration expressed in E fTis the mass normalized thrust generated by the propellers and e3 0 0 1 T Authors in 12 have shown that the blade fl apping drag force acting on a quadrotor are proportional to the velocity of the quadrotor expressed in B and the sum of propeller speeds Hence the mass normalized drag force fdcan be expressed as follows fd vb 7 2784 4 X i 1 i 8 where iis the speed of the ithpropeller Since the quadrotor performs slow maneuvers it can be assumed that the pro pellers rotates at hovering speeds Hence can be approx imated as a constant Mass and propeller speed normalized drag coeffi cient matrix Kd is defi ned as Kd m diag k 9 where k are the drag coeffi cients and m is the mass of the quadrotor Acceleration of the quadrotor in 6 is expressed in body frame B to facilitate the analysis and the estimator design The second order terms appear due to coordinate transfor mation are neglected similar to work in 8 and the equation for vbcan be expressed as vb fd fTe3 CTg 10 In general MAVs are not equipped with sensors to mea sure the thrust generated by the propellers Therefore the thrust fT is defi ned as a state and thrust rate uTfrom the trajectory controller is considered as an input for the system Full system dynamics of the quadrotor MAV can be expressed in the form of x f x u as x Cvb fd fTe3 CTg R q T0 T uT 11 where x pTvT b qTfT T is the state vector of the system and u TuT T is the control input B Sensors Acceleration of the quadrotor is measured using the on bord IMU and the accelerometer measurement with zero mean Gaussian noise a can be expressed as yacc Kdvb fTe3 a 12 Angular velocity of the body frame is measured through the gyroscope and the measurement can be expressed as ygyro 13 where is the zero mean Gaussian noise of the measure ment Similar to simplifi cation proposed in 3 this study assumes that accelerometer and gyroscope are initially cali brated and assumes the random walk biases are negligible Position vector between the quadrotor and the ithrange beacon can be expressed as ri p piwhere piis the location of the ithanchor expressed in E It is assumed that piis known for all the beacon Hence the range measurement to the ith anchor can be defi ned as ri kp pik r 14 where ris the measurement noise modeled as zero mean Gaussian noise IV OBSERVABILITYANALYSIS In this section we use the nonlinear observability rank condition proposed in 17 to conduct the observability anal ysis on the quadrotor dynamics given in 11 for scenarios S1 and S2 Control affi ne form of a general nonlinear system x f x u can be expressed as x f0 x X k 1 l fk x uk y h x 15 where f0is the zero input function of the process model and fkcorresponds to the function that is excited by the kth component of the input control vector u Authors in 17 state that a non linear system is locally weakly observable if the nonlinear observability matrix O has full rank O is defi ned using the Lie derivatives of h x with respect to fk x Following the notation in 7 the nonlinear observability matrix O can be expressed as O Llfa fbh x a b 0 k l N 16 The quadrotor dynamics shown in 11 can be rearranged and expressed in the control affi ne form as x Cvb Kdvb fTe3 CTg 0 0 z f0 x 0 0 1 2R q 0 z f1 x 0 0 0 0 1 z f2 x uT 17 Rest of this section will analyze the system under each scenario and determine the observability conditions of the system To represent the equations in a more concise manner x is omitted from h x f0 x f1 x and f2 x in the Lie derivatives A Observability conditions for S1 Scenario S1 is the case where three range measurements are available along with accelerometer measurements to estimate the pose of the quadrotor MAV In the observability analysis r2 i 2 is chosen instead of ri in the measurement model for the easiness of the calculation Since riand r2 i 2 are strictly positive and have a one to one correspondence between them both provide the same information and does not affect the observability of the system The measurement model for the scenario S1 can be expressed as h x Kdvb fTe3 1 2r 2 1 1 2r 2 2 1 2r 2 3 Kdvb fTe3 1 2 p p1 T p p1 1 2 p p2 T p p2 1 2 p p3 T p p3 18 2785 Lie derivatives of h x with respect to f0 x can be ex pressed as L0h h L0h 03 3 Kd03 4 e3 R303 303 403 1 R3 r1r2r3 T L1 f0h L 0hf 0 Kd h Kdvb fTe3 CTg i R3Cvb L1 f0h 03 3K2 d q CTg Kde3 V3CTR3CR3 q Cvb 03 1 V3 vbvbvb T q Cvb Cvb q 19 First order Lie derivative of h with respect to f1is zero L1 f1h 06 4 and fi rst order Lie derivative with respect to f2is a constant L1 f2h eT 3 01 3 T Therefore the gradient of L1 f1h and L 1 f2h are zero and not included in the observability matrix The nonlinear observability matrix for the S1 scenario can be expressed as OS1 L0hT L1 f0h T T 20 Assume that the quadrotor is not on a line con necting two beacons i e ri6 krj k R i j 1 2 3 ThenOS1hasfullrank Theblockmatrix q CTg T R3 q Cvb T T loses its column rank of 4 when the quadrotor is stationary i e vb 0 or the quadrotor is moving in the direction of gravity i e Cvb kg k R Hence the orientation of the system becomes unobservable under above conditions If the quadrotor is on a line connecting two beacons then the block matrix RT 3 CV T 3 T loses rank when the quadrotor moves towards a beacon i e Cvb lri l R i 1 2 3 As a result the positions become unobservable In addition if the quadrotor is stationary while on a line connecting two beacons the observability matrix loses two ranks and the position and ori entation become unobservable For all the other conditions OS1 has rank of 11 and satisfi es the condition required for the system to be locally weakly observable B Observability conditions for S2 The measurement model for the scenario S2 with two range measurements can be expressed as h x Kdvb fTe3 1 2r 2 1 1 2r 2 2 21 Nonlinear observability matrix OS2 is defi ned as follows OS2 L0hT L1 f0h T L2 f0f0h T T 22 Derivations of zeroth and fi rst order Lie derivatives for scenario S2 can be derived similar to scenario S1 and therefore not shown here Second order Lie derivative with respect to f0 and its gradient can be expressed as L2 f0h L 1h f0f0 K2 d h Kdvb fTe3 CTg i V2vb R2C h Kdvb fTe3 CTg i R2 r1r2 T V2 vbvb T L2 f0f0h 03 3 K3 d K2 d q C Tg K2 de3 Vb2CT2V2 R2CKdR2 q C vb R2Ce3 Vb2 vb vb T vb Kdvb fTe3 CTg 23 When the quadrotor is stationary all the terms contain ing vb vbvanishes and position and orientation become unobservable Similar to scenario S1 if the quadrotor is moving in the direction of gravity then the OS2loses a rank and orientation becomes unobservable In addition if the quadrotor is moving towards a range beacon Cvb kri OS2 k R i 1 2 loses a rank in the fi rst block column i e the position becomes unobservable In all the other conditions OS1has full rank of 11 and the system is locally weakly observable Table I shows the summarized results of the completed observability analysis TABLE I SUMMERY OF THE OBSERVABILITY ANALYSIS ScenarioObservabilityUnobservable Conditions S1 vb 0 Cvb kg ri krj Cvb lri S2 vb 0 Cvb kg Cvb lri Note System is locally weakly observable and System is unobservable k l R i j 1 2 3 V STATEESTIMATOR In this study an extended Kalman fi lter EKF is used to estimate the states of the quadrotor MAV For brevity the standard EKF equations available in literature are not repeated here The process model of the fi lter comprises of the dynamic model shown in 11 and can be expressed as p Cvb q 1 2R q g T0 T vb fd fTe3 CTg m fT uT u 24 The angular velocity of the MAV and the thrust rate from the trajectory controller is used as the inputs for the system Hence the gyroscope measurement noise gis incorporated as process noise Also a noise term mis introduced to 2786 compensate for any un modeled dynamics of the quadrotor MAV Measurements from accelerometer and range sensor are used for the update step of the EKF Since this paper consider two scenarios with two and three range measurement there are two measurement models The general measurement model can be expressed as follows h x yacc a ri r 25 VI RESULTS A Simulation Results A numerical simulator was devel