DEVELOPMENT OF A MINIATURE LOW-COST UAV HELICOPTER AUTOPILOT PLATFORM AND FORMATION FLIGHT CONTROL OF UAV TEAMS YUN BEN (B.Eng and M.Eng, Harbin Institute of Technology, China) A THESIS SUBMITTED FOR THE DEGREE OF DOCTOR OF PHILOSOPHY DEPARTMENT OF ELECTRICAL & COMPUTER ENGINEERING NATIONAL UNIVERSITY OF SINGAPORE 2010 Summary Unmanned aerial vehicles (UAVs) have become a popular research topic worldwide recently. Their important role and their great potential are being explored continuously. Among various UAVs, small UAV helicopter has special attractiveness to the academic circle, due to its small size, unique flight capacities, outstanding maneuverability, and low cost. Nowadays with the fast development in manufacturing skill and martial science, constructing a small-scale UAV helicopter which is upgraded from advanced hobby-purpose helicopter becomes feasible and affordable. Its unlimited potential for various practical implementations motivates our NUS UAV research team to carry out a comprehensive study and exploration. One of our goals is to develop a miniature UAV helicopter platform and construct several UAVs as test beds towards implementing multiple UAV team formation flight. With limited research budget, low-cost miniature UAV helicopter was chosen as our research platform. Developing an autonomous miniature UAV helicopter is a challenge for several reasons. Firstly, helicopters are inherently unstable and capable of exhibiting high acceleration rates. They are highly sensitive to control inputs and require high frequency feedback with minimum delay for stability. Secondly, the dynamics of helicopters are unstable, multivariable, highly coupled, which is thus hard for modeling. Thirdly, helicopters have limited on-board power and payload capacity. Flight control systems must be compact, efficient, and light weight for effective on-board integration. As the saying goes “the lower the price, the worse quality of the goods”, smaller and lower cost sensors also mean lower performance, which makes the things even harder. Finally, helicopters are extremely dangerous. Our research on miniature UAV helicopters started from the year 2003. The overall design and control procedure for the miniature UAV is similar as the small-scale UAV helicopters, which includes: (1) hardware construction, (2) software system development, (3) dynamic modeling, and (4) control law design and implementation. Besides these, with a low-cost micro-size Inertial Measurement Unit (IMU) and a miniature UAV helicopter, we need thoroughly calibrate sensors, optimize our Global Position System (GPS) information and also self-develop attitude determination algorithm. In spite of the difficulties, one of the advantages of choosing a miniature UAV helicopter as the platform lies in that the experiment with a miniature helicopter is simple: it is easy to find a small area for testing i Summary ii and thus can greatly reduce our previous research time. This is the reason why we are able to extend our research in the formation flight. The fundamental requirement for carrying our UAV research is having reliable miniature UAV helicopters in hand. In the first year, we have constructed and tested the miniature UAV helicopter. One compact with special design , for constructing a miniature UAV helicopter platform with minimum payload and lowest cost has been summarized and proposed. After the autopilot system hardware was integrated, software was developed to ensure all of the hardware components work properly. Several software necessaries such as sensor data collection and filtering, control signal collection, and data storage were implemented on it. Since a dynamic model is needed before designing the controller, based on the classic modeling method, we first developed a linear multi-input multi-output (MIMO) model through time-domain and frequency-domain system identification. To avoid flight crash due to temporary GPS signal block or failure, a special robust cascaded modeling and control method was proposed. The autopilot system performs very well in hovering and low speed flying in real flight experiment. With the reliable UAV helicopter platform at hand, we then further extended our research in the formation flight field. We present a full scheme for the formation flight of multiple UAV helicopters. Moreover, we adopt the leader-follower pattern to maintain a fixed geometrical formation while navigating the UAVs following certain trajectories. More specifically, the leader is commanded to fly on some predefined trajectories, and each follower is controlled to maintain its position in formation using the measurement of its inertial position and the information of the leader’s position and velocity, obtained through a wireless Compact Flash (CF) card. More specifications are made for multiple UAV formation flight. In order to avoid possible collisions of UAV helicopters in the actual formation flight test, a collision avoidance scheme based on some predefined alert zones and protected zones is employed. Simulations and real flight experimental results have verified that our design is effective. To conclude this work, we summarize the research achievements and contribution, then provide the meaningful research directions for future. Acknowledgements “The important thing in life is to have a great aim, and the determination to attain it”. Johan Wolfgang von Goethe, (German Poet and dramatist). It is beyond doubt that the work in this thesis can not be completed without the support, advice, and encouragement of others, teachers, colleagues, friends, and family members. In this acknowledgement, I wish to formally acknowledge their support and pay special thanks to the role they have played during the course of this project. First and foremost, I would like to gratefully and sincerely thank Prof. Ben M. Chen for his guidance, understanding, and patience in all aspects of this research during my studies at National University of Singapore. I also owe a debt of gratitude to Dr. Kai-Yew Lum and Prof. T. H. Lee for their assistance and counsel, and for the generous time and invaluable insights that they shared with me in reviewing my work, as well as for having served as my academic advisor throughout this endeavor. The members of the UAV group have contributed immensely to my personal and professional time at NUS. The group has been a source of friendships as well as good advice and collaboration. I have had the pleasure to work with all of them. Special thanks to Mr. Alvin Cai, who has explored the avionics in his final year project and Dr. Guowei Cai who has explored the MIMO system modeling. I am especially grateful for them: Dr. Kemao Peng, Dr. Miaobo Dong, Mr. Feng Lin and Mr. Xiangxu Dong. To my family, for their reassuring confidence in the inevitable conclusion of this work, in particular I would like to thank my mother for her unwavering faith in me, my father who I deeply admire, and my brothers for their strong support, of which I’m truly grateful. Finally I would like to thank the two most important people in my life, my wife and daughter. You mean more to me than life itself. Thank you for your love, support, encouragement, and most importantly, laughter. Thank you. iii Contents Summary i Acknowledgements iii Contents iv List of Figures viii List of Tables xi Nomenclature xii Introduction 1.1 General Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Project Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Technical Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.1 Coordinate Frames Definition . . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Attitude Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.3 Actuator Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 Goals and Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 1.5 Outline of this Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 iv CONTENTS v Systematic Construction of a Miniature UAV Platform 2.1 2.2 2.3 2.4 The UAV System Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.1.1 Bare Helicopter: TREX 450 . . . . . . . . . . . . . . . . . . . . . . . . 14 2.1.2 Avionic System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.1.3 Servo Control Module . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.1.4 Other Issues Related to the UAV Design . . . . . . . . . . . . . . . . . 28 Software Development . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2.2.1 Onboard Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 2.2.2 The Ground Station: a Monitoring Program . . . . . . . . . . . . . . 33 Ground and Flight Test Evaluation . . . . . . . . . . . . . . . . . . . . . . . . 35 2.3.1 EMI Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 2.3.2 Vibration Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 2.3.3 Manual Flight Test . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 Robust GPS Enhancement and Attitude Determination 3.1 3.3 3.4 3.5 41 Sensors Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.1.1 3.2 13 Magnetometer Hard and Soft Iron Calibration . . . . . . . . . . . . . 43 Typical Filtering Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 3.2.1 Complimentary Filtering Algorithm . . . . . . . . . . . . . . . . . . . 45 3.2.2 Kalman Filtering Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 48 3.2.3 Extended Kalman filtering Algorithm . . . . . . . . . . . . . . . . . . 50 3.2.4 Discrete-time H∞ Filtering . . . . . . . . . . . . . . . . . . . . . . . . 52 3.2.5 Comparison of the Filtering Algorithms . . . . . . . . . . . . . . . . . 54 Atttitude Determination . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 3.3.1 Determination of Aircraft Attitude . . . . . . . . . . . . . . . . . . . . 55 3.3.2 H∞ Filtering for Euler-angles Determination . . . . . . . . . . . . . . 58 3.3.3 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 GPS Signal Enhancement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 3.4.1 Position Signals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 3.4.2 H∞ Filtering for GPS Position Signal Enhancement . . . . . . . . . . 69 3.4.3 Position Determination Experimental Results . . . . . . . . . . . . . . 69 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 CONTENTS vi Dynamic Modeling and Control of the Miniature UAV Helicopter 4.1 4.2 4.3 4.4 4.5 4.6 73 Data Collection and Preprocessing . . . . . . . . . . . . . . . . . . . . . . . . 74 4.1.1 Select the Input Signals . . . . . . . . . . . . . . . . . . . . . . . . . . 74 4.1.2 Collect Flight Test Data . . . . . . . . . . . . . . . . . . . . . . . . . . 76 4.1.3 Preprocessing of the Raw Dataset . . . . . . . . . . . . . . . . . . . . 80 Helicopter Aerodynamics Model Structure . . . . . . . . . . . . . . . . . . . . 81 4.2.1 Degree of Freedom (DOF) Rigid-body Dynamics . . . . . . . . . . . 82 4.2.2 Coupled Rotor Flapping Dynamics . . . . . . . . . . . . . . . . . . . . 82 4.2.3 Yaw Rate Gyro Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 83 MIMO Modeling and Control Method . . . . . . . . . . . . . . . . . . . . . . 84 4.3.1 Modeling of BabyLion UAV Helicopter . . . . . . . . . . . . . . . . . . 84 4.3.2 Control Law Design for BabyLion UAV Helicopter . . . . . . . . . . . 91 Cascaded Modeling and Control Method . . . . . . . . . . . . . . . . . . . . 94 4.4.1 Cascaded Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95 4.4.2 Control Law Design for BabyLion UAV Helicopter . . . . . . . . . . . 101 Simulation and Implementation Results . . . . . . . . . . . . . . . . . . . . . 104 4.5.1 MIMO Control Method . . . . . . . . . . . . . . . . . . . . . . . . . . 104 4.5.2 Cascaded Control Implementation Results . . . . . . . . . . . . . . . . 107 4.5.3 Comparison of the Two Control Methods . . . . . . . . . . . . . . . . 113 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118 Formation Control Modeling, Control Law Design and Implementation 120 5.1 Model of a Leader-Follower Formation Flight . . . . . . . . . . . . . . . . . . 121 5.2 Control of Formation Flight . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 5.3 5.4 5.5 5.2.1 Dynamic Inversion Control Law for Outer-loop Control of the Follower UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 5.2.2 Leader-Follower Formation Flight Input Constraint . . . . . . . . . . . 128 5.2.3 Multiple UAV Formation Flight . . . . . . . . . . . . . . . . . . . . . . 129 Collision Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 5.3.1 Collision Avoidance for Two UAV Case . . . . . . . . . . . . . . . . . 136 5.3.2 Multiple UAV Group Collision Avoidance . . . . . . . . . . . . . . . . 142 5.3.3 Obstacle Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 Formation Flight Test Result . . . . . . . . . . . . . . . . . . . . . . . . . . . 145 5.4.1 Formation Flight Experiment Setup . . . . . . . . . . . . . . . . . . . 146 5.4.2 Test Result . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148 conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151 CONTENTS vii Conclusions 153 6.1 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154 6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155 A Publication/Submitted Paper List 158 List of Figures 1.1 The UAV helicopter, BabyLion. . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 UAV helicopter family in NUS. . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Reference frames. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 The formation frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.5 Definition of normal Euler angles. . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Basic helicopter - TREX 450 . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 2.2 The avionics system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.3 Functional system architecture [31]. . . . . . . . . . . . . . . . . . . . . . . . . 17 2.4 MNAV100CA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 2.5 Stargate development platform (processor board and daughter card). . . . . . 20 2.6 AmbiCom wireless 802.11 card. . . . . . . . . . . . . . . . . . . . . . . . . . . 22 2.7 Auto mode switch on RC transmitter. . . . . . . . . . . . . . . . . . . . . . . 25 2.8 PPM signal and its decoded output. . . . . . . . . . . . . . . . . . . . . . . . 25 2.9 MNAV mode switching function. . . . . . . . . . . . . . . . . . . . . . . . . . 26 2.10 A typical receiver circuit diagram with PPM decoding. . . . . . . . . . . . . . 26 2.11 A typical receiver print circuit board (PCB) with PPM decoding. . . . . . . . 27 2.12 UAV avionics. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 2.13 Soft model architecture [31]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 2.14 Scheduling time line. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 2.15 Ground station GUI with NUS campus map. . . . . . . . . . . . . . . . . . . 34 2.16 UAV in flight. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 2.17 Manual hovering flight test. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 3.1 Calibration example: accelerometer offset, scale factor compensation. . . . . . 42 viii LIST OF FIGURES ix 3.2 Effects of soft/ hard-iron distortions. . . . . . . . . . . . . . . . . . . . . . . . 44 3.3 Magnetic calibration readings. . . . . . . . . . . . . . . . . . . . . . . . . . . . 44 3.4 Rate gyro performance. Solid: the true attitud; Dash: integrated output of the rate gyro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 3.5 Accelerometer performance. solid: the true attitude; Dash: the accelerometer estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 3.6 Attitude angle estimation using accelerometer. . . . . . . . . . . . . . . . . . 47 3.7 Comparison of Euler angles estimated by H∞ filter (solid line) and those estimated by NAV420 (dashed line). . . . . . . . . . . . . . . . . . . . . . . . 63 3.8 The measured body-frame y-axis position and velocity during a hovering flight from GPS. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.9 The position integrated by velocity (bold line) and that measured by GPS receiver (thin line). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 3.10 Comparison of the x-y position estimated by H∞ filter (bold line) and that measured by NAV100CA (thin line). . . . . . . . . . . . . . . . . . . . . . . . 70 3.11 Comparison of the position estimated by H∞ filter (solid line) and that measured by NAV100CA (dashed line). . . . . . . . . . . . . . . . . . . . . . . . . 71 4.1 Typical frequency sweep input signal. . . . . . . . . . . . . . . . . . . . . . . 76 4.2 Typical doublet input signal. . . . . . . . . . . . . . . . . . . . . . . . . . . . 77 4.3 Input signals in the yaw channel perturbation experiment. . . . . . . . . . . . 78 4.4 Position outputs in the yaw channel perturbation experiment. . . . . . . . . . 78 4.5 Velocity outputs in the yaw channel perturbation experiment. . . . . . . . . . 79 4.6 Angular rates in the yaw channel perturbation experiment. . . . . . . . . . . 79 4.7 Euler angles in the yaw channel perturbation experiment. . . . . . . . . . . . 80 4.8 An illustration for tip-path-plane. . . . . . . . . . . . . . . . . . . . . . . . . . 83 4.9 Verification of the identified model. . . . . . . . . . . . . . . . . . . . . . . . . 90 4.10 General flight control scheme for UAVs. . . . . . . . . . . . . . . . . . . . . . 91 4.11 General flight control scheme for UAVs. . . . . . . . . . . . . . . . . . . . . . 98 4.12 Verification of the identified model. . . . . . . . . . . . . . . . . . . . . . . . . 100 4.13 Outputs of hovering flight simulation. . . . . . . . . . . . . . . . . . . . . . . 105 4.14 Control inputs of hovering flight simulation. . . . . . . . . . . . . . . . . . . . 106 4.15 Outputs of automatic hovering flight test. . . . . . . . . . . . . . . . . . . . . 106 4.16 Control inputs of automatic hovering flight. . . . . . . . . . . . . . . . . . . . 107 CHAPTER 3. 57 where wx is the process noise. It can be shown (see, for example, [41]) that the Euler angles are given by 2(q2q3 + q0 q1 )  2    −1 − 2(q1 + q2 )   θ  =  sin [−2(q1 q3 − q0 q2 )]      2(q q + q q ) ψ tan−1 − 2(q22 + q32 )  φ   tan−1 (3.32) In principle, the gyroscope alone is capable of providing attitude information through integration. However, the estimation error resulted from integration increases as time progresses because of the drift. As such, an attitude determination reference independent of the gyroscope have to be used to correct the resulting error. This can be done by using the measurement signals from accelerometers and magnetometers available in INS as follows. 1. It can be shown that the measurement of the accelerations in the axes of the aircraft body frame can be expressed as  ax   − sin θ   −2g(q1q3 − q0 q2 )         ay  = −g  cos θ sin φ  + amotion =  −2g(q0q1 + q2 q3 )  + amotion       az (3.33) −g[1 − 2(q12 + q22 )] cos θ cos φ where the first term is due to the gravity force (g is the gravity constant) and the second term, i.e., amotion, is related to flight motio. amotion is negligible compared to the gravity acceleration for small UAVs, and can generally be treated as a measurement noise or disturbance. 2. The magnetometer provides a reliable measurement for the heading angle ψ by using ψ = tan−1 mxh myh (3.34) where mxh and myh are projected magnetic field components on the horizontal plane that can be calculated by transforming the magnetometer measurement vector m as CHAPTER 3. follows, 58    mx = mx cos(φ) + my sin(θ) sin(φ) − mz cos(θ) sin(φ) h   myh = my cos(θ) + mz sin(θ) (3.35) where mx , my and mz are measured components of magnetic field vector m along x, y, and z axes of body frame respectively. This measured ψ, which corresponding to the third equation in (3.32) can be included as the fourth measurement output for heading angle update. We combine (3.31), the third equation of (3.32), and (3.33) to formulate the determination framework for the Euler angles. After the estimation of q0 − q3 , we determine the Euler angles using (3.32). These equations are normally used for the EKF algorithm to estimate aircraft attitude. 3.3.2 H∞ Filtering for Euler-angles Determination For the nonlinear Euler-angles determination dynamics, we first rewrite its continuous-time dynamics obtained in Section 3.3.1 as follows:    x˙ = fc (x) + w    z = hc (x) + v      s=x (3.36) where x ∈ Rn is the state variable, z ∈ Rp is the measurement output, fc (x) and hc (x) are sufficiently smooth functions of appropriate dimension, w and v are process and measurement noises or disturbances, assumed to be deterministic and with bounded energy. Its corresponding discrete-time counterpart can be expressed as    xk = f (xk−1 ) + Gwk    zk = h(xk ) + V vk      sk = xk (3.37) CHAPTER 3. 59 Expanding the nonlinear functions f (·) and h(·) using Taylor series expansion at the filtered x ˆk−1 and predicted estimates x ¯k−1 , we obtain    xk = f (ˆ xk−1 ) + Fk (xk−1 − x ˆk−1 ) + Gwk   zk = h(¯ xk ) + Hk (xk−1 − x ¯k−1 ) + V vk (3.38) where Fk and G are the Jacobian matrices of partial derivatives of f with respect to x and w, Hk , and V are the Jacobian matrices of partial derivatives of h with respect to x and v. Fk , Hk , V and G is given in (3.15). Without knowing the individual values of the noise wk and vk at each time step, the state and measurement vector can be approximated as    x ¯k = f (ˆ xk−1 )   z¯k = h(¯ xk ) (3.39) We then substitute (3.39) into (3.38), and define the new state vector x ˜k as    x ˆk−1 (previous step) ˜k−1 = xk−1 − x   x ˜k = xk − x ¯k (current step) (3.40) Note that for constructing x ˜k−1 , we use x ˆk−1 instead of x ¯k−1 since the former is the more accurate time-update result. Define a new measurement output z˜k as    z˜k = V −1 (zk − z¯k )  ˜ k = V −1 Hk  H (3.41) CHAPTER 3. 60 With (3.40) and (3.41), we can give the H∞ -compatible formulation as    ˜k−1 + Gwk x ˜k = Fk x    ˜kx z˜k = H ˜k + vk      s˜k = x ˜k (3.42) Finally, we note that the corresponding Jacobian matrices for the attitude dynamics are given by  −(ωx − bp)T    (ωx − bp)T    (ω − b )T  y q   Fk =  (ωz − br )T         −(ωy − bq )T −(ωz − br )T q1 T q2 T (ωz − br )T −(ωy − bq )T −q0 T q3 T −(ωz − br )T (ωx − bp)T −q3 T −q0 T (ωy − bq )T (ωx − bp)T q2 T −q1 T q3 T    q2 T    q1 T     , −q0 T          where T is the sampling period, which is taken as 0.02 s in obtaining the implementation results given in the next section,  2gq2 −2gq3 2gq0 −2gq1    −2gq1 −2gq0 −2gq3 −2gq2  Hk =   2gq2 −2gq3  −2gq0 2gq1  q3 δ2δ3 q2 δ2 δ3 q1 δ2 δ3 + 2q2 δ1 δ3 q0 δ2 δ3 + q3 δ1 δ3 0    0     0   0 where δ1 = 2(q1 q2 + q0 q3 ), δ2 = − 2(q22 + q32 ), δ3 = [1 − 2(q22 + q32)]2 , + [2(q1q2 + q0 q3 )]2 CHAPTER 3. 61  and          G=         10−4 10−4 10−4 10−4 10−6 10−6 10−6      V =    0.981 0.981 0.981 0.1221           ,              .    The choice G and V needs special attention. Improper ratios of G and V may lower the estimation accuracy or convergence rate. For example, as the gyro bias is normally regarded to have smaller variation than the angular rate, the corresponding value for the gyro bias in the the Jacobian matrices G is about 1/10 of that for the angular rate (quaternions). Similarly, the magnetometer measurement is regarded to have smaller variation than the accelerometer, and hence the corresponding value for the magnetometer measurement in the the Jacobian matrices V is smaller than that for the accelerometer measurement. The G and V values may vary due to the different inertial sensors used. Based on the dynamics given in (3.42), an H∞ filter can be constructed as the following. 1. State prediction: x ¯k = f (ˆ xk−1 ) (3.43) CHAPTER 3. 62 2. Time update:   ˜ T (I + H ˜ k Pk H ˜ T)−1  Kk = Pk H  k k   x ˆk = x ¯k + Kk (zk − h(¯ xk ))     −1 T −1  ˜ TH ˜ −1 ˜ T − γ −2I Pk+1 = [F˜k (Pk−1 + H k k ) Fk + GG ] (3.44) where Pk is the corresponding covariance matrix of xk , and Kk is the resulting H∞ filter gain. It should be noted that it is not possible to use a constant γ for this problem ˜ k are time-varying. A specific γ is required to be as the Jacobian matrices F˜k and H determined in real-time and used for each step. As for the initial states, we take P0 = diag{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1} and x0 = 0. 3.3.3 Experimental Results In this section, we evaluate the practical performance of the designed H∞ filters. To prove the efficiency of our design, we compare the estimation result with the output data of a commercial product, namely, an NAV420 from Crossbow. Such a product has a GPS-aided AHRS navigation system developed through years of extensive application experience. Its attitude and position estimation is based on Crossbow’s self-developed Extended Kalman filter algorithm. The result for the Euler-angles determination is depicted in Fig. 3.7. It can be noted that with the H∞ filter, the responses of the attitude estimation are faster. Furthermore, it provide a faster estimation than the algorithm adopted by NAV420. The estimated result is also flatter when the UAV is stationary, while the attitude estimation from NAV420 needs longer time to converge. The result indicates that the obtained H∞ filter for Euler-angles determination is very satisfactory. CHAPTER 3. 63 NAV420 H∞ estimation 20 15 φ (deg) 10 −5 −10 −15 20 40 60 80 100 120 140 Time (s) 160 180 200 220 160 180 200 220 160 180 200 220 a. Roll angle 15 10 θ (deg) −5 −10 −15 −20 20 40 60 80 100 120 140 Time (s) b. Pitch angle 200 150 100 ψ (deg) 50 −50 −100 −150 −200 20 40 60 80 100 120 140 Time (s) c. Yaw angle Figure 3.7: Comparison of Euler angles estimated by H∞ filter (solid line) and those estimated by NAV420 (dashed line). CHAPTER 3. 3.4 64 GPS Signal Enhancement To obtain accurate position and navigation signals, we usually employ a GPS sensor unit which is capable of providing position information of the UAV. Unfortunately, performance obtained from low-cost inertial sensors available in the market is pretty poor due to error sources such as random noise, biases, and scale factor errors, etc Even though expensive and bulky INS are able to provide accurate navigation data, their performance degrades gradually with time (see, for example, [8] and [58]). Similarly, navigation data generated by GPS sensors carry bounded errors. The GPS signals for positions use only have an accuracy of about m with a sampling frequency of Hz. The velocities-determined from the Doppler effect-have an accuracy of about 0.3 m/s. These measurement errors are rather bad for mini-scale UAVs flying at low speed. As such, it is necessary to introduce some filtering schemes to smooth and improve the GPS measurements for control purposes and the attenuation of high frequency noises. There have been a number of approaches recently introduced to improving the performance of GPS and low-cost INS integration. For example, Nassar et al [48] have investigated the improvement of the accuracy of an IMU using an autoregressive modeling approach. Different integration filters have also been investigated in the literature. For instance, Shin and El-Sheimy [54] and van der Merwe and Wan [46] have studied the use of unscented Kalman filters. The unscented Kalman filter (UKF) uses a deterministic sampling technique known as the unscented transform to pick a minimal set of sample points (called sigma points) around the mean. These sigma points are then propagated through the non-linear functions, from which the mean and covariance of the estimate are then recovered. The result is a filter which more accurately captures the true mean and covariance. Noureldin et al [50] has considered solutions using neural networks. More traditional approaches of improving the measurement accuracy of GPS and low-cost INS reported in the literature include increasing the number of measurements used in the Kalman filter. CHAPTER 3. 65 The goal here is aimed to present an integration of a position reference system for a mini UAV helicopter by utilizing the robust and H∞ filtering technique. 3.4.1 Position Signals In this section, we outline an aircraft position enhanement framework, which is to be used for further H∞ filtering design in next section. We begin with the problem that motivates us to carry out such an enhancement. During the flight tests of our UAV helicopter, we have constantly experienced some unexpected jumps in position signals received from the GPS receiver (see Fig. 3.8, and also in [61]). However, we have never observed such jumps occurred in the actual system or even from the velocity signals measured (also see Fig. 3.8). Since the GPS velocity signals received are relatively more accurate compared to those of GPS positions, we wish to evaluate whether the GPS velocity could provide a reasonably accurate position reference by simply integrating 3-axis velocity. However, due to unknown slow-varying bias, a simple integration generally results large errors with a large time frame. Fig. 3.9 shows the comparison of the GPS position signals and the results calculated through integration. Such a problem that we have encountered in experimental testing of our UAV helicopter suggests that there is a need to process the raw data received from the GPS in order to improve the quality of signals received and the overall performance of our flight control systems. In what follows, we derive a set of equations which can be utilized for the reconstruction of the position signals through the integration of filtered velocities with corrections based position signals received from the GPS receiver. Normally in practical situations, the measurement noise can be modeled as a white noise. Furthermore, there are some low-frequency bias contained in the measured position values. Based on this situation, we can describe the GPS-measured velocity signal vx as follows vx = vx∗ + bx + wx (3.45) CHAPTER 3. 66 −1 −1.5 py (m) −2 −2.5 −3 −3.5 −4 20 40 60 Time (s) 80 100 120 80 100 120 a. Position signal. 0.3 0.25 vy (m/s) 0.2 0.15 0.1 0.05 −0.05 20 40 60 Time (s) b. Velocity signal. Figure 3.8: The measured body-frame y-axis position and velocity during a hovering flight from GPS. where vx∗ is the true velocity, bx is a slow varying bias, and wx is the white noise. The corresponding position can then be computed as px = vx dt = (vx∗ + bx + wx)dt = p∗x + (bx + wx)dt (3.46) where p∗x is the true position. Noting that bx is a slow varying variable, we take it as an CHAPTER 3. 67 Velocity integration Measured position px (m) −2 −4 −6 −8 −10 −12 50 100 150 200 250 Time (s) a. Body-frame x-axis position signal. 20 Velocity integration Measured position 15 py (m) 10 −5 −10 50 100 150 200 250 Time (s) b. Body-frame x-axis position signal. Velocity integration Measured position 30 20 pz (m) 10 −10 −20 −30 −40 50 100 150 200 250 Time (s) c. Body-frame z-axis position signal. Figure 3.9: The position integrated by velocity (bold line) and that measured by GPS receiver (thin line). CHAPTER 3. 68 unknown constant with appropriate noise, i.e., b˙ x = wb (3.47) where wb is assumed to be white noise. Define ex to be the position error, i.e., ex = px − p∗x . It follows from (3.46) that ex = (bx + wx )dt . (3.48) Thus, e˙x = bx + wx (3.49) Next, consider the received position signal, i.e., px,gps = p∗x + nx (3.50) where nx is the position measurement noise. We rewrite px,gps = (px − ex ) + nx = px − ex + nx . (3.51) Defining a measurement output y = px − px,gps , we have y = ex − nx (3.52) Putting (3.47) and (3.49) into a matrix form, we have x˙ = 0 x+w (3.53) where x= ex bx and w= wx wb (3.54) CHAPTER 3. 69 The measurement output can then be written as y = [ ] x − nx . (3.55) We combine (3.53) and (3.55) together for single-axis position determination. It should be noted that the above formulation is applicable for all the axes of the aircraft. 3.4.2 H∞ Filtering for GPS Position Signal Enhancement For the GPS position signal enhancement, we discretize the continuous-time system of (3.16) with a sampling period of 0.02 s. Such a sampling period is used in the hardware system of our UAV helicopter. We obtain            0.02   0.02  xk =    xk−1 +   wk    0.002 (3.56)   zk = [ ]xk + vk        sk = xk The H∞ filtering technique described in (3.17) and (3.18) is then implemented. Using the result of [16], we can compute that the best possible choice of γ is given by γ ∗ = 1.04897 for this system. However, it requires an infinite gain to achieve such noise attenuation. As for practical filtering design, we choose a γ = 1.23 > γ ∗, which yields a satisfactory performance. Note that the initial values for P0 and x0 are chosen by     P0 =  , 3.4.3     x0 =   . (3.57) Position Determination Experimental Results In this section, we evaluate the practical performance of the designed H∞ filters. To prove the efficiency of our design, we compare the estimation result with the raw data of MAV CHAPTER 3. 70 35 GPS Raw Data H∞ estimation 30 px (m) 25 20 15 10 10 15 20 25 30 35 40 p (m) y Figure 3.10: Comparison of the x-y position estimated by H∞ filter (bold line) and that measured by NAV100CA (thin line). 100CA which is directly collected from the GPS receiver. Among various flight tests conducted for the GPS signal enhancement, we select a triangle-path flight test as a sample to evaluate the performance of the H∞ filter we design. The comparison results are shown in Fig. 3.10 and Fig. 3.11. The results clearly show that with the enhancement of the GPS signals, the problem of jumping in the position signals has been successfully resolved. Furthermore, high frequency noises in the GPS position signals are also totally eliminated. CHAPTER 3. 71 45 H∞ estimation GPS Raw Data 40 35 px (m) 30 25 20 15 10 20 40 60 80 100 Time (s) 120 140 160 180 140 160 180 140 160 180 a. Body-frame x-axis position signal. 35 30 py (m) 25 20 15 10 0 20 40 60 80 100 Time (s) 120 b. Body-frame y-axis position signal. −28 −29 pz (m) −30 −31 −32 −33 −34 20 40 60 80 100 Time (s) 120 c. Body-frame z-axis position signal. Figure 3.11: Comparison of the position estimated by H∞ filter (solid line) and that measured by NAV100CA (dashed line). CHAPTER 3. 3.5 72 Conclusion We learnt from our experience after the unsuccessful half-year attempt that the poor performance could be attributed to the bad sensor information without calibration and filtering. In this chapter, sensor calibration is first introduced. After that, a low cost attitude determination and navigation optimization system has been investigated in this chapter. We obtained a better attitude measurement with fast response and no estimation bias using the recently developed H∞ filtering technique. The experimental results showed that such a scheme is very effective. The design has been implemented and used on our UAV system to provide reliable measurement for conducting various automatic flight control systems. This filtering algorithm is also suitable for other various aircrafts. [...]... also has special attractiveness to the academic circle because of their smaller size, expendable lower cost, outstanding maneuverability, ease of uses, and wide range of capabilities The smaller size UAV helicopters are commonly upgraded from radio-controlled hobby helicopters by assembling an avionic system They are one of the best platforms applied as typical plants for academic research as they can... Other software capability includes sensor information processing such as sensor filtering and optimization CHAPTER 1 3 While a single autonomous UAV can be very useful in performing various tasks, multiple UAVs operating as a team to accomplish a given task cooperatively may offer even greater advantages in certain applications, such as in target search and detection in a large area of coverage With a mini -UAV. .. highly dynamic behavior of helicopter, limited payload capability and lack of highly accurate sensors, miniature UAVs are still rare Designing a miniature UAV helicopter is a challenging job, there are many aspects that need to be thoroughly considered with the special constraints, such as: 1 hardware components selection which is limited by the budget and also the payload capability; 2 modeling and control. .. makes the helicopter fly forward or backwards, i.e., pitch motion Lateral plane is a plane parallel to the YABC − ZABC plane Longitudinal plane is a plane parallel to the XABC − ZABC plane 1. 4 Goals and Objectives The goal of this project is first to build a miniature UAV platform The appropriate parts which range from the IMU, on-board computer to power systems have to be chosen and integrated On board... levels of performance depending on their applications UAVs range in size from the man-portable (mini- and micro-UAVs), which are powered by electricity and usually weigh less than 2 kg, small-scale UAV which are powered by oil or gasoline and typically weighs 10 -50 kg, to full-size aircraft (large size UAV) Among various UAVs, the large UAVs are used mostly for military purposes In contrast, the 1 CHAPTER... CHAPTER 1 2 smaller, tactical UAVs are being developed to support tactical units to perform very short range “over the hill” and “around the corner” reconnaissance, and assist in protection While each mission requires a different profile and capabilities, the man portable Miniature Aerial Vehicles (MAVs) are designed to provide reasonably good performance at an a ordable price Miniature UAV helicopter also... interfaces, also the PPM (Pulse-position modulation) input interface and sensor calibration in the internal EEPROM The advantage of this is that because this product is specifically made for miniature robotic platforms The two key requirements: navigation and servo control are satisfied in one package thus saving space and weight compared to having a separate GPS aided inertial system and servo control board... derivative magnetometer soft calibration x-axis ratio magnetometer soft calibration x-axis bias Body frame x-axis speed derivative Body frame y-axis spring derivative Body frame y-axis rotor spring derivative Body frame y-axis rotor spring derivative formation frame y-axis spring derivative NED frame y-axis spring derivative magnetometer soft calibration y-axis ratio magnetometer soft calibration y-axis bias... design and implementation We adopt the leader-follower pattern to maintain a fixed geometrical formation while navigating the UAVs following certain trajectories More specifications are made for multiple UAV formation flight A collision avoidance scheme based on some predefined alert zones and protected zones is also proposed Simulations and real flight experimental results are finally shown in detail Finally,... frame to leader’s body frame bias in the GPS velocity information Output matrix in linearized model structure Forward error in the formation frame Forward clearance in the formation frame The acceleration of gravity Heave error in the formation frame Heave clearance in the formation frame Moment of inertia matrix Rolling moment of inertia Pitching moment of inertia Yawing moment of inertia Main shaft . DEVELOPMENT OF A MINIATURE LOW- COST UAV HELICOPTER AUTOPILOT PLATFORM AND FORMATION FLIGHT CONTROL OF UAV TEAMS YUN BEN (B.Eng and M.Eng, Harbin Institute of Technology, China) A THESIS. target search and detection in a large area of coverage. With a mini -UAV platform being built, it is natural for us to expand our research domain to the formation flight of multiple UAVs, as formation. performance at an a ordable price. Miniature UAV helicopter also has special attractiveness to the academic circle because of their smaller size, expendable lower cost, outstanding maneuverability,