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 AbstractThis 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. Johns, NL, Canada. A1B 3X5. email:hctef2, oscar.desilva,gmann,rgosinemun.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, 911 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 = qxqyqzqwT= ? 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 = xyzTis 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 = 033 Kd034e3 R3033034031 R3= r1r2r3T L1 f0h = L 0hf 0= Kd h Kdvb fTe3+ CTg i R3Cvb L1 f0h = 033K2 d q(CTg)Kde3 V3CTR3CR3q(Cvb)031 V3= vbvbvbT q(Cvb) = (Cvb) q (19) First order Lie derivative of h with respect to f1is zero ? L1 f1h = 064 ? and fi rst order Lie derivative with respect to f2is a constant ? L1 f2h = ?eT 3 013?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= ? L0hTL1 f0h T ?T (20) Assume that the quadrotor is not on a line con- necting two beacons, i.e. ri6=krj,kR,i,j= 1,2,3.ThenOS1hasfullrank.Theblockmatrix, ?( q(CTg)T(R3q(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= ? L0hTL1 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= r1r2T V2= vbvbT L2 f0f0h = 033K3 d K2 dq(C Tg) K2 de3 Vb2CT2V2 R2CKdR2q(C vb)R2Ce3 Vb2= vb vbT 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