uavbook supplement January 22, 2019 Small Unmanned Aircraft

Transcription

1 uavbook supplement January 22, 2019 Small Unmanned Aircraft

2 uavbook supplement January 22, 2019

3 uavbook supplement January 22, 2019 Small Unmanned Aircraft Theory and Practice Supplement Randal W. Beard Timothy W. McLain PRINCETON UNIVERSITY PRESS PRINCETON AND OXFORD

4 uavbook supplement January 22, 2019

5 uavbook supplement January 22, 2019 Contents 1. Introduction 1 2. Coordinate Frames Design Project 3 3. Kinematics and Dynamics State Variables Kinematics Rigid-body Dynamics Chapter Summary Design Project 5 4. Forces and Moments Gravitational Forces Aerodynamic Forces and Moments Propulsion Forces and Moments Atmospheric Disturbances Chapter Summary Design Project 19 A. Quaternions 21 A.1 Another look at complex numbers 21 A.2 Introduction to Quaternions 22 A.3 Quaternion Rotations 22 A.4 Conversion Between Euler Angles and Quaternions 24 A.5 Conversion Between Quaternions and Rotation Matrices 25 A.6 Quaternion Kinematics 26 A.7 Aircraft Kinematic and Dynamic Equations 26 B. Simulation Using Object Oriented Programming 29 B.1 Introduction 29 B.2 Numerical Solutions to Differential Equations 29 C. Animation 37 C.1 3D Animation Using Python 37 C.2 3D Animation Using Matlab 37

6 vi CONTENTS C.3 3D Animation Using Simulink 37 Bibliography 41 Index 43

7 uavbook supplement January 22, 2019 Chapter One Introduction

8 uavbook supplement January 22, 2019

9 uavbook supplement January 22, 2019 Chapter Two Coordinate Frames NOTES AND REFERENCES 2.1 DESIGN PROJECT The objective of this assignment is to create a 3D graphic of a MAV that is correctly rotated and translated to the desired configuration. Creating animations in Matlab/Simulink/Python is described in Appendix C and example files are contained on the textbook website. 2.1 Read Appendix C and study carefully the spacecraft animation given at the textbook website. 2.2 Create an animation drawing of the aircraft shown in Figure Create a simulation that verifies that the aircraft is correctly rotated and translated in the animation. 2.4 In the animation file, switch the order of rotation and translation so that the aircraft is first translated and then rotated, and observe the effect.

10 4 COORDINATE FRAMES (a) Airframe 10 fuse_l1 fuse_l3 fuse_l2 wing_l tail_h fuse_h (b) Side view tailwing_l fuse_w fuse_l2 fuse_l1 wing_l fuse_l3 wing_w tailwing_l tailwing_w (c) Top view Figure 2.1: Specifications for animation of aircraft for the project.

11 uavbook supplement January 22, 2019 Chapter Three Kinematics and Dynamics 3.1 STATE VARIABLES 3.2 KINEMATICS 3.3 RIGID-BODY DYNAMICS 3.4 CHAPTER SUMMARY NOTES AND REFERENCES 3.5 DESIGN PROJECT 3.1 Implement the MAV equations of motion given in Equations (??) through (??). Assume that the inputs to the block are the forces and moments applied to the MAV in the body frame. Changeable parameters should include the mass, the moments and products of inertia, and the initial conditions for each state. Use the parameters given in Appendix??. 3.2 Verify that the equations of motion are correct by individually setting the forces and moments along each axis to a nonzero value and convincing yourself that the motion is appropriate. 3.3 Since J xz is non-zero, there is gyroscopic coupling between roll and yaw. To test your simulation, set J xz to zero and place nonzero moments on l and n and verify that there is no coupling between the roll and yaw axes. Verify that when J xz is not zero, there is coupling between the roll and yaw axes.

12 uavbook supplement January 22, 2019

13 uavbook supplement January 22, 2019 Chapter Four Forces and Moments 4.1 GRAVITATIONAL FORCES Using a unit quaternion to represent attitude instead of Euler angles, the gravity force can be expressed as fg b = mg 2(e x e z e y e 0 ) 2(e y e z + e x e 0 ). e 2 z + e 2 0 e2 x e 2 y 4.2 AERODYNAMIC FORCES AND MOMENTS Control Surfaces Longitudinal Aerodynamics Lateral Aerodynamics Aerodynamic Coefficients 4.3 PROPULSION FORCES AND MOMENTS Based on an intuitive understanding of how propellers function, it is reasonable and correct to conclude that the thrust produced by a propeller depends on the angular speed of the propeller and the speed at which the propeller is moving through the surrounding air mass, or in other words the airspeed V a of the aircraft. The dependence of propeller performance on the propeller speed with respect to the airspeed is captured by a non-dimensional parameter called the advance ratio, which for a propeller is given by J = V a nd = 2πV a ΩD, (4.1) where n is the propeller speed in revolutions per second, Ω is the propeller speed in radians per second, and D is the propeller diameter. It is common to characterize propeller performance empirically by plotting propeller torque and thrust coefficients, C Q and C T, versus the advance ratio as shown in figures 4.1 and 4.2. The non-dimensional coefficients C Q and C T are defined

14 8 FORCES AND MOMENTS as Q p C Q = ρn 2 D 5 (4.2) T p C T = ρn 2 D 4, (4.3) where Q p is propeller torque, T p is propeller thrust, and ρ is the density of air. Because the thrust produced by the propeller is proportional to C T, it is clear from figure 4.2 that for a fixed propeller speed Ω, the thrust varies significantly with advance ratio J and thus with airspeed V a C Q, data and 2nd-order curve fit C Q = ( ) J 2 + ( ) J + ( ) advance ratio - J (1/rev) Figure 4.1: Torque coefficient versus advance ratio for APC Thin Electric 10x5 propeller [?]. In modeling the propulsion system for the MAV, we assume that the propeller is driven by a battery-powered electric motor with a variable voltage input. From a modeling perspective, the challenge is to determine the operating speed of the motor/propeller combination given the properties of the motor and propeller, the applied motor voltage, and the airspeed of the aircraft. Once we know the operating speed of the motor and propeller, we can calculate the torque produced by the motor and the thrust produced by the propeller. Intuitively, if we apply a constant voltage to a motor, it will spin up to a steady-state speed where the torque generated in the motor rotor is equal to the aerodynamic torque acting on the propeller. The motor torque is dependent on the motor properties and the speed of the motor. The aerodynamic

15 Forces and Moments C T, data and 2nd-order curve fit C T = ( ) J 2 + ( ) J + ( ) advance ratio - J (1/rev) Figure 4.2: Thrust coefficient versus advance ratio for APC Thin Electric 10x5 propeller [?]. torque on the propeller is dependent on its aerodynamic properties, the forward airspeed of the propeller, and the angular speed of the propeller. We will develop equations for motor torque and propeller torque versus motor/propeller speed and equate them, solving for the operating speed of the motor and propeller. Let s start with the motor. For a DC motor, the steady-state torque generated for a given input voltage V in is given by [ ] 1 Q p = K Q R (V in K V Ω) i 0, (4.4) where K Q is the motor torque constant, R is the resistance of the motor windings, K V is the back-emf voltage constant, Ω is the angular speed of the motor, and i 0 is the zero-torque or no-load current. Both K Q and K T represent how power is transformed between the mechanical and electrical energy domains. If consistent units (such as SI units) are utilized, K Q and K T are identical in value. 1 It should be noted that when a voltage change is applied to the motor, the motor changes speed according to the dynamic behavior of the motor. For a fixed-wing aircraft, these transients are significantly faster than the aircraft dynamics and we can neglect them without negatively effecting the accuracy of our model. 1 We assume K Q has units of N-m/A and K V has units of V-sec/rad. In RC aircraft motor datasheets, it is common for K V to be specified in units of rpm/v.

16 10 FORCES AND MOMENTS To model how propeller torque is related to propeller speed, we need an analytical expression that relates the two. We can develop this relationship by fitting a quadratic equation to the torque-coefficient data in Figure 4.1. Doing so yields an equation that relates the torque coefficient to the advance ratio for a specific propeller C Q C Q2 J 2 + C Q1 J + C Q0. (4.5) From the definition of the non-dimensional propeller torque coefficient in Equation (4.2), we have Q p = ρn 2 D 5 C Q ρn 2 D 5 ( C Q2 J 2 + C Q1 J + C Q0 ), (4.6) where ρ is air density and J is the advance ratio defined in (4.1). Recognizing that n = Ω/(2π), we can express n and J in terms of angular velocity and substitute to get ( Q ρd 3 C Q2 Va 2 + D ) 2π C Q1V a Ω + D2 (2π) 2 C Q0Ω 2, (4.7) which is a quadratic equation relating propeller torque to propeller speed. By equating the motor torque in (4.4) with the propeller torque in (4.7), we can form an expression that is quadratic in the propeller speed. Solving this equation will give the propeller speed for which the motor torque and propeller torque are in equilibrium. This will be the operating speed of the propeller for a given aircraft airspeed and motor voltage input. Equating (4.4) and (4.7) gives ( ρd 5 (2π) 2 C Q0 ) ( ρd Ω π C Q1V a + K QK V R ) Ω ( ρd 3 C Q2 V 2 a K Q R V in + K Q i 0 Denoting the coefficients of this quadratic equation as a = ρd5 (2π) 2 C Q0 b = ρd4 2π C Q1V a + K QK V R c = ρd 3 C Q2 V 2 a K Q R V in + K Q i 0, ) = 0. (4.8)

17 Forces and Moments 11 the positive root given by Ω op = b + b 2 4ac 2a defines the operating speed of the propeller for the specified airspeed V a and motor voltage V in. We now assume that the non-dimensional thrust coefficient C T can also be approximated by a quadratic function of the advance ratio as C T C T 2 J 2 + C T 1 J + C T 0. (4.9) Figure 4.2 shows an example of this approximation as determined from experimental data for a specific propeller. If we define the operating advance ratio corresponding to the operating speed of propeller as J op = 2πV a Ω op D, (4.10) we can use (4.3) to calculate the propeller thrust to be where ρω 2 opd 4 T p = C T,op (2π) 2, (4.11) C T,op = C T 2 J 2 op + C T 1 J op + C T 0. This propeller thrust specified by (4.11) is the thrust produced for a specified motor voltage V in and airspeed V a for a specific motor/propeller combination and their physical properties. The corresponding propeller torque can be most easily calculated from the motor-torque equation (4.4) as [ ] 1 ( ) Q p = K Q Vin K V Ω op i0. R The approach of equating the motor torque and propeller torque expressions and solving for the operating speed and torque can be depicted graphically as shown in figure 4.3. Using the approach above to find the operating speed, the thrust and torque characteristics of the motor/propeller combination over the full range of voltage and airspeed inputs can be quantified. A family of curves, one for each level of voltage input, is plotted versus airspeed and shows the thrust and torque generated by a specific motor/propeller pair in figures 4.4 and 4.5. Although the voltages are plotted at discrete intervals, the voltage input is assumed to be continuously variable. Figure 4.4 in particular illustrates

18 <latexit sha1_base64="ski9xkzzvgk1h6akkuijctaoxoe=">aaab7nicbza9swnbeibnosyyv6kwnotbsjbwz6onelcxtmb8qhkevc0mwbk7d+zocehij7cxumtw32nn509x81fo4gsld+/msdnvlehh0fe/vnzg5lz+u7bt3n3bpzgshr03bzwaxhsslrfpr9ryktrvoedj24nhvewst6lx3azeeutgilg/4cthoajdlqacuxrwq05usb2neqwyx/hniusqlkfcztflnwdueqxpbj9mqeiamatwdgi/wtcjbgwtffrsppynli3pkhccaqq4dbp5ulny7pw+gctgpy1k7v6eykiydqii16kojuxqbwb+v+ukolgjm6gtfllmi48gqsqyk9ntpc8mzygndigzwu1k2igaytalvhqhbksnr0pzqhi4rrs0lmghapzcgvxaandqhxuoqqmyjoejxudvs7xn7817x7tmvoxmcfyr9/edht6qeq==</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="ck8pdc+ekzh4numsp+zg7r8leyk=">aaab2xicbzdnsgmxfixv1l86vq1rn8eiucozbnqpuhfzwbzco5rm5k4bmskmyr2hdh0bf25efc93vo3pz0jbdwq+zknivscullqubn9ebwd3b/+gfugfnfzjk9nmo2fz0gjsilzl5jnmfpxu2cvjcp8lgzylffbj6f0i77+gstlxtzqrmmr4wmtuck7o6oyaraadlmw2ivxdc9yanb+gss7kddujxa0dhefbucunsafw7g9liwuxuz7ggupnm7rrtrxzzi6dk7a0n+5oykv394ukz9bostjdzdhn7ga2mp/lbiwlt1eldvesarh6kc0vo5wtdmajnchizrxwyasblykjn1yqa8z3hysbg29d77odon4moa7ncafxemin3meddkalahj4hxdv4r15h6uuat66tdp4i+/zbzjgijg=</latexit> <latexit sha1_base64="mht3zvm0zrumyeyyftlywigqsjc=">aaab43icbzblswmxfixv+ky1anxrjlgev2xgjw4ewy3lfuwd2qfk0jttajijsuyoq3+egxek+j/c+w9mhwttprd4occh955ec25dgh4hw9s7u3v7pypyyexo+kr6wmnbldcmwywtmekm1klgcluoo4fdbzdkrganmtzm884zgssz9esmgmnjr4qnnfhnru6t3jhmqa6qtbaelkq2ivpbdvzqdkpf/whgconkmugt7uwhdnfbjenm4kzczy1qyiz0hd2pikq0cbeyd0yuvtmkawb8uy4s3n8vciqtncre35tuje16njf/y3q5s2/jgiudo1rs+vgac+iymt+ddllb5stua2wg+1kjg1ndmfmnlx0j0frkm9c+rkeemygu4bwu4aoiuif7eiqgtidbbf7gdd4dhbwgh8u6tojvb2fwr8hnd9tujvi=</latexit> <latexit sha1_base64="mht3zvm0zrumyeyyftlywigqsjc=">aaab43icbzblswmxfixv+ky1anxrjlgev2xgjw4ewy3lfuwd2qfk0jttajijsuyoq3+egxek+j/c+w9mhwttprd4occh955ec25dgh4hw9s7u3v7pypyyexo+kr6wmnbldcmwywtmekm1klgcluoo4fdbzdkrganmtzm884zgssz9esmgmnjr4qnnfhnru6t3jhmqa6qtbaelkq2ivpbdvzqdkpf/whgconkmugt7uwhdnfbjenm4kzczy1qyiz0hd2pikq0cbeyd0yuvtmkawb8uy4s3n8vciqtncre35tuje16njf/y3q5s2/jgiudo1rs+vgac+iymt+ddllb5stua2wg+1kjg1ndmfmnlx0j0frkm9c+rkeemygu4bwu4aoiuif7eiqgtidbbf7gdd4dhbwgh8u6tojvb2fwr8hnd9tujvi=</latexit> <latexit sha1_base64="f73kpdoljiqxneddoyi1axyrexe=">aaab7nicbzbnswmxeizn61etx1wpxojf8cbl14tehiixjy3yd2ixkk3tnjtjlsmsujb+cc8efphq7/hmvzft96ctlwqe3pkhm2+usghr97+9wsbm1vzocbe0t39wefq+pmnzodwmn1ksy9ojqovsan5egzj3esopiirvr5p7eb39xi0vsx7eacjdrudadawj6kx2g9yrrl/1yxw/6i9e1ihioqk56v3yv28qs1rxjuxsa7ubn2cyuyocst4r9vlle8omdms7djvv3ibzyt0zuxdogaxj455gsnb/t2ruwttvketufmd2tty3/6t1uxzehpnqsypcs+vhw1qsjmn8djiqhjouuweugef2jwxmdwxoeiq5eilvk9ehdv0nhdf8su0qj6miz3aolxdaddtgaerqbayteizxepms78v79z6wrquvnzmfp/i+fwdzj46r</latexit> <latexit sha1_base64="f73kpdoljiqxneddoyi1axyrexe=">aaab7nicbzbnswmxeizn61etx1wpxojf8cbl14tehiixjy3yd2ixkk3tnjtjlsmsujb+cc8efphq7/hmvzft96ctlwqe3pkhm2+usghr97+9wsbm1vzocbe0t39wefq+pmnzodwmn1ksy9ojqovsan5egzj3esopiirvr5p7eb39xi0vsx7eacjdrudadawj6kx2g9yrrl/1yxw/6i9e1ihioqk56v3yv28qs1rxjuxsa7ubn2cyuyocst4r9vlle8omdms7djvv3ibzyt0zuxdogaxj455gsnb/t2ruwttvketufmd2tty3/6t1uxzehpnqsypcs+vhw1qsjmn8djiqhjouuweugef2jwxmdwxoeiq5eilvk9ehdv0nhdf8su0qj6miz3aolxdaddtgaerqbayteizxepms78v79z6wrquvnzmfp/i+fwdzj46r</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="t/2d+rbqpmbb7abz+aoq+fp0ab0=">aaab7nicbzc7sgnbfibpeokx3qkwfo4gwuldro02qsdgmgfzgwqjs5pzzmhclplzisx5cbslrwx9hjvfwwdwcik08yebj/+cw5zzrwlnxvr+l7eyurae28hvfra2d3b3ivshdanstwidkk50k8kgcizp3tllasvrfiui02y0vjvum49ug6bkgx0lnbs4l1nmclboatbqlap1rbdy8sv+vggzgjmukrkg/z4+uax2i5+dnikponisjo1pb35iwwxrywin40innttbzij7to1qykfnme3xhamz5/rqrlr70qkp+3siw8kykyhcp8b2ybzre/o/wju18u2ymzmklkoy+yhoobiktw5hpaypsxzkabpn3k6idldgxlqeci6eyphkzwhclqphnzfgbcyuhym4hxmi4boqca9vqaobitzbc7x6iffsvxnvs9yvbz5zch/kffwamb2q+g==</latexit> <latexit sha1_base64="f73kpdoljiqxneddoyi1axyrexe=">aaab7nicbzbnswmxeizn61etx1wpxojf8cbl14tehiixjy3yd2ixkk3tnjtjlsmsujb+cc8efphq7/hmvzft96ctlwqe3pkhm2+usghr97+9wsbm1vzocbe0t39wefq+pmnzodwmn1ksy9ojqovsan5egzj3esopiirvr5p7eb39xi0vsx7eacjdrudadawj6kx2g9yrrl/1yxw/6i9e1ihioqk56v3yv28qs1rxjuxsa7ubn2cyuyocst4r9vlle8omdms7djvv3ibzyt0zuxdogaxj455gsnb/t2ruwttvketufmd2tty3/6t1uxzehpnqsypcs+vhw1qsjmn8djiqhjouuweugef2jwxmdwxoeiq5eilvk9ehdv0nhdf8su0qj6miz3aolxdaddtgaerqbayteizxepms78v79z6wrquvnzmfp/i+fwdzj46r</latexit> 12 FORCES AND MOMENTS 0.4 motor prop torque (N-m) 0.1 Q = Q m <latexit sha1_base64="zkt8chv0ddo+dpnlvezqw9daeeg=">aaab+nicbza7swnbemfn4ivg10vlm8mgwei4s9eyygmjiwaekiswt5kks/ye7m6p4cxhsbfqxnzpymfnr3hzkdtxdws//jpdzp79wapnrvtlzvzw19y3spu5re2d3t07v1/tuai4vnkki9xwmuypqqysiimnwcelfil1f3g5qdfvugkrhbc0iredsh4oeoizmlbhzrfkafzzp0x4qgkuj3mdu+aw3amczfdmucjz1+vvakh07m9wn+jjgcfxybruem5m7zqpelziondknmamd1kfmwzdfqbup9ptx86xcbpol1lmherm3d8tkqu0hgw+6qwydfribwl+v2sm1ltopykme8kqzxb1eulq5exyclpcisc5msc4euzwhw+yypxmwpmqvmuvl0ptrogzvjfpnmjmwtieizgbd86hbfdqgspwuicneifx69f6tt6s91lrxprphmafwr8/pcivrq==</latexit> sha1_base64="xrbj6bxjkfe5v8uo4taynfojgl4=">aaab+nicbzc7sgnbfizn4y3g20zlm8ugwejytvgwcdjysckycyzlmj2ctybmxpg5q4y1j2jjoyitt2jn5as4m6tqxb8gpv5zdufm78wck7ttlyo3tlyyupzfl2xsbm3vmmxdhoosyadoihhjlkcvcb5chtkkamusaoajahrdi6zevaopebte4cggn6d9kpucudrw1yx2qgh0abed8ibpfi8lxbnkl+2jrevwzlcqmffv7/peba1rfnz6eusccjejqltbswn0uyqrmwhjqidrefm2ph1oawxpampnj6eprupt9cw/kvqfae3c3xmpdzqabz7udcgo1hwtm/+rtrp0z9yuh3gcellpij8rfkzwlopv4xiyipegyitxt1psqcvlqnpkqndmv7wijzoyo/lap3fmpsqtfxjajohdtkmfxjiaqrng7sktesgvxqpxblwz79pwndgb2sn/zhz8apeglqu=</latexit> sha1_base64="lm2e0x41pgfwlt9sjamc1f/zufu=">aaab+nicbzdlssnafiynxmu9pbp0eyyccymjg10w3lizgr1ae8jketionvyyovfl7ko4cagiw5/enw/jpm1cw38y+pjpozwzf5akrtc2v42v1bx1jc3kvnv7z3dv36wddfsssqztlohe9gkqqpay2shrqc+vqknaqdcyxxx17j1ixzp4dicpebedxjzkjkk2flpm3kqwpl6l8ih5kk6rvlm3g/zm1ji4jdrjqzzvfrmdhgurxmgevarv2cl6ozximybp1c0upjsn6rd6gmmagfly2elt60q7aytmph4xwjp390roi6umuaa7i4ojtvgrzp9q/qzdsy/nczohxgy+kmyehylv5ganuasgyqkbmsn1rrybuukz6rskejzfly9d57zhal61682zmo4koslh5jq45ii0ytvpktzh5ie8k1fyzjwzl8a78tfvxthkmupyr8bndzlxk+e=</latexit> op motor/prop speed (RPM) Figure 4.3: Motor and propeller torque plotted versus angular speed. The intersection of these two curves represents the equilibrium operating speed of the motor/propeller combination for a specific voltage command and airspeed, in this case V a = 15 m/s and V = 10 V. the full range of thrust performance for the selected motor/propeller pair. As expected, the maximum thrust for a given voltage setting occurs at zero airspeed and the thrust produced goes down as the airspeed increases. Note that these plots model the windmilling effect that can occur at excessively high advance ratios, where both the thrust and torque become negative and the propeller produces drag instead of thrust. This condition can occur when an aircraft is gliding and descending in a motor-off state and the airspeed is being controlled using the pitch angle of the aircraft. Assuming that the center of the propeller is on the body frame x-axis and that the direction of the propeller is also aligned with the body frame x-axis, then the force and torque due to the propeller is given by f p = ( T p, 0, 0 ) m p = ( Q m, 0, 0 ). 4.4 ATMOSPHERIC DISTURBANCES In this section, we will discuss atmospheric disturbances, such as wind, and describe how these disturbances enter into the dynamics of the aircraft. In Chapter 2, we defined V g as the velocity of the airframe relative to the

19 Forces and Moments thrust (N) 12 V 5 10 V 8 V 6 V 4 V 0 2 V airspeed (m/s) Figure 4.4: Propeller thrust versus airspeed for various motor voltage settings V V torque (N-m) V V 4 V 0 2 V airspeed (m/s) Figure 4.5: Motor torque versus airspeed for various motor voltage settings.

20 14 FORCES AND MOMENTS ground, V a as the velocity of the airframe relative to the surrounding air mass, and V w as the velocity of the air mass relative to the ground, or in other words, the wind velocity. As shown in Equation (??), the relationship between ground velocity, air velocity, and wind velocity is given by V g = V a + V w. (4.12) For simulation purposes, we will assume that the total wind vector can be represented as V w = V ws + V wg, where V ws is a constant vector that represents a steady ambient wind, and V wg is a stochastic process that represents wind gusts and other atmospheric disturbances. The ambient (steady) wind is typically expressed in the inertial frame as Vw i s = w n s, w es w ds where w ns is the speed of the steady wind in the North direction, w es is the speed of the steady wind in the East direction, and w ds is the speed of the steady wind in the Down direction. The stochastic (gust) component of the wind is typically expressed in the aircraft body frame because the atmospheric effects experienced by the aircraft in the direction of its forward motion occur at a higher frequency than do those in the lateral and down directions. The gust portion of the wind can be written in terms of its bodyframe components as Vw b g = u w g. v wg w wg Experimental results indicate that a good model for the non-steady gust portion of the wind model is obtained by passing white noise through a linear time-invariant filter given by the von Karmen turbulence spectrum in [1]. Unfortunately, the von Karmen spectrum does not result in a rational transfer function. A suitable approximation of the von Karmen model is

21 Forces and Moments 15 Table 4.1: Dryden gust model parameters [?]. altitude L u = L v L w σ u = σ v σ w gust description (m) (m) (m) (m/s) (m/s) low altitude, light turbulence low altitude, moderate turbulence medium altitude, light turbulence medium altitude, moderate turbulence given by the Dryden transfer functions 2Va 1 H u (s) = σ u L u s + Va ( H v (s) = σ v 3Va L v H w (s) = σ w 3Va L w s + ( L u ) Va 3Lv s + Va L v ) 2 ( ) s + Va 3Lw ( ) 2, s + Va L w where σ u, σ v, and σ w are the intensities of the turbulence along the vehicle frame axes; and L u, L v, and L w are spatial wavelengths; and V a is the airspeed of the vehicle. The Dryden models are typically implemented assuming a constant nominal airspeed V a0. The parameters for the Dryden gust model are defined in MIL-F-8785C. Suitable parameters for low and medium altitudes and light and moderate turbulence were presented in [?] and are shown in Table 4.1. Figure 4.6 shows how the steady wind and atmospheric disturbance components enter into the equations of motion. White noise is passed through the Dryden filters to produce the gust components expressed in the vehicle frame. The steady components of the wind are rotated from the inertial frame into the body frame and added to the gust components to produce the total wind in the body frame. The combination of steady and gust terms can be expressed mathematically as Vw b = u w = R b v(φ, θ, ψ) w n s + u w g, v w w w w es w ds v wg w wg where R b v is the rotation matrix from the vehicle to the body frame given in Equation (??). From the components of the wind velocity V b w and the

22 16 FORCES AND MOMENTS Figure 4.6: The wind is modeled as a constant wind field plus turbulence. The turbulence is generated by filtering white noise with a Dryden model. ground velocity Vg, b we can calculate the body-frame components of the airspeed vector as Va b = u r v r = u u w v v w. w r w w w From the body-frame components of the airspeed vector, we can calculate the airspeed magnitude, the angle of attack, and the sideslip angle according to Equation (??) as V a = u 2 r + vr 2 + wr 2 ( ) α = tan 1 wr u r ( ) β = sin 1 v r. u 2 r + vr 2 + wr 2 These expressions for V a, α, and β are used to calculate the aerodynamic forces and moments acting on the vehicle. The key point to understand is that wind and atmospheric disturbances affect the airspeed, the angle of attack, and the sideslip angle. It is through these parameters that wind and atmospheric effects enter the calculation of the aerodynamic forces and moments and thereby influence the motion of the aircraft.

23 Forces and Moments 17 Note that to implement the system Y (s) = as + b s 2 + cs + d U(s), first put the system into control canonical form ( ) ( c d 1 ẋ = x + u 1 0 0) y = ( a b ) x, and then convert to discrete time using x k+1 = x k + T s (Ax k + Bu k ) y k = Cx k where T s is the sample rate, to get x k+1 = ( 1 Ts c T s d T s 1 y k = ( a b ) x k. ) x k + ( ) Ts u 0 k For the Dryden model gust models, the input u k will be zero mean Gaussian noise with unity variance. Python code that implements the transfer function above is given as follows: import numpy a s np c l a s s t r a n s f e r f u n c t i o n : def i n i t ( s e l f, Ts ) : s e l f. t s = Ts # s e t i n i t i a l c o n d i t i o n s s e l f. s t a t e = np. a r r a y ( [ [ 0. 0 ], [ 0. 0 ] ] ) # d e f i n e s t a t e space model s e l f. A = np. a r r a y ([[1 Ts c, Ts d ], [ Ts, 1 ] ] ) s e l f. B = np. a r r a y ( [ [ Ts ], [ 0 ] ] ) s e l f. C = np. a r r a y ( [ [ a, b ] ] ) def u p d a t e ( s e l f, u ) : Update s t a t e space model s e l f. s t a t e = s e l f. s e l f. s t a t e + s e l f. B u y = s e l f. s e l f. s t a t e return y # i n i t i a l i z e t h e s y s t e m Ts = # s i m u l a t i o n s t e p s i z e system = t r a n s f e r f u n c t i o n ( Ts )

24 18 FORCES AND MOMENTS # main s i m u l a t i o n loop s i m t i m e = 0. 0 while s i m t i m e < : u=np. random. randn ( ) # ( w h i t e n o i s e ) y = system. u p d a t e ( u ) # u pdate based on c u r r e n t i n p u t s i m t i m e += Ts # i n c r e m e n t t h e s i m u l a t i o n t i m e 4.5 CHAPTER SUMMARY The total forces on the MAV can be summarized as follows: f x mg sin θ f y = mg cos θ sin φ + T p 0 f z mg cos θ cos φ C X (α) + C Xq (α) c 2 ρv a 2 2V a q S b b C Y0 + C Yβ β + C Yp 2V a p + C Yr 2V a r C Z (α) + C Zq (α) c 2V a q + 1 C Xδe (α)δ e 2 ρv a 2 S C Yδa δ a + C Yδr δ r, (4.13) C Zδe (α)δ e where C X (α) = C D (α) cos α + C L (α) sin α C Xq (α) = C Dq cos α + C Lq sin α C Xδe (α) = C Dδe cos α + C Lδe sin α (4.14) C Z (α) = C D (α) sin α C L (α) cos α C Zq (α) = C Dq sin α C Lq cos α C Zδe (α) = C Dδe sin α C Lδe cos α, and where C L (α) is given by Equation (??) and C D (α) is given by Equation (??). The subscripts X and Z denote that the forces act in the X and Z directions in the body frame, which correspond to the directions of the i b and the k b vectors.

25 Forces and Moments 19 The total torques on the MAV can be summarized as follows: [ ] l b b b C m = 1 l0 + C lβ β + C lp 2V a p + C lr 2V a r [ ] n 2 ρv a 2 c S c C m0 + C mα α + C mq 2V a q [ ] b b b C n0 + C nβ β + C np 2V a p + C nr 2V a r + 1 b [ ] C lδa δ a + C lδr δ r 2 ρv a 2 S c [ ] C mδe δ e b [ ] + Q p 0. (4.15) C nδa δ a + C nδr δ r 0 NOTES AND REFERENCES 4.6 DESIGN PROJECT 4.1 Add simulation of the wind to the mavsim simulator. The wind element should produce wind gust along the body axes, and steady state wind along the NED inertial axes. 4.2 Add forces and moments to the dynamics of the MAV. The inputs to the MAV should now be elevator, throttle, aileron, and rudder. The aerodynamic coefficients are given in Appendix??. 4.3 Verify your simulation by setting the control surface deflections to different values. Observe the response of the MAV. Does it behave as you think it should?

26 uavbook supplement January 22, 2019

27 uavbook supplement January 22, 2019 Appendix A Quaternions A.1 ANOTHER LOOK AT COMPLEX NUMBERS Let z = z r + jz i and w = w r + jw i be two complex numbers. Since j 2 = 1, multiplication gives zw = (z r + jz i )(w r + jw i ) = z r w r + j 2 z i w i + j(z i w r + w i z r ) = (z r w r z i w i ) + j(z i w r + w i z r ). In an alternate universe, instead of imaginary numbers, mathematicians might have work solely with vectors in R 2, if they had defined vector multiplication appropriately. Suppose now that z = (z r, z i ) R 2 and w = (w r, w i ) R 2, and that vector multiplication in R 2 were defined as follows: ( ) ( ) ( ) ( ) zr wr zr z = i wr z i w i z i z r w i ( ) zr w = r z i w i z i w r + w i z r = M(z)w, where M(z) = ( ) zr z i. z i z r Note that defining vector multiplication in this way is identical to multiplying two complex numbers. Note also, that if the complex number is on the unit circle, i.e., z = e jθ = cos θ + j sin θ that the associated matrix is ( ) cos θ sin θ M(z) =, sin θ cos θ i.e., a complex number on the unit circle, represents a rotation of vector in 2D.

28 22 QUATERNIONS A.2 INTRODUCTION TO QUATERNIONS Quaternions are an extension of complex numbers to four dimensions. A quaternion can be represented as where i, j, k satisfy e = e 0 + ie x + je y + ke z, i 2 = j 2 = k 2 = ijk = 1. These relationships also imply that ij = k, jk = i, and ki = j. Using these expressions it can be shown that qe = (q 0 e 0 q x e x q y e y q z e z ) + i(q x e 0 + q 0 e x + q z e y q y e x ) + j(q y e 0 q z e x + q 0 e y + q x e z ) + k(q z e 0 + q y e x q x e y + q 0 e z ). If instead, we represent quaternions as elements of R 4, then quaternion multiplication can be defined as qe = M(q)e, where q 0 q x q y q z M(q) = q x q 0 q z q y q y q y q 0 q x. q z q y q x q 0 A.3 QUATERNION ROTATIONS As complex numbers on the unit circle, represent rotations in 2D, similarly, unit quaternions, i.e., quaternions such that e = 1 can be used to represent rotations in 3D. Quaternions provide an alternative way to represent the attitude of an aircraft. While it could be argued that it is more difficult to visualize the angular motion of a vehicle specified by quaternions instead of Euler angles, there are mathematical advantages to the quaternion representation that make it the method of choice for many aircraft simulations. Most significantly, the Euler angle representation has a singularity when the pitch angle θ is ±90 deg. Physically, when the pitch angle is 90 deg, the roll and yaw angles are indistinguishable. Mathematically, the attitude kinematics specified by Equation (??) are indeterminate since cos θ = 0 when θ = 90 deg. The quaternion representation of attitude has no such singularity. While

29 Quaternions 23 this singularity is not an issue for the vast majority of flight conditions, it is an issue for simulating aerobatic flight and other extreme maneuvers, some of which may not be intentional. The other advantage that the quaternion formulation provides is that it is more computationally efficient. The Euler angle formulation of the aircraft kinematics involves nonlinear trigonometric functions, whereas the quaternion formulation results in much simpler linear and algebraic equations. A thorough introduction to quaternions and rotation sequences is given by Kuipers [2]. An in-depth treatment to the use of quaternions specific to aircraft applications is given by Phillips [3]. In its most general form, a quaternion is an ordered list of four real numbers. We can represent the quaternion e as a vector in R 4 as e = e 0 e x e y e z, where e 0, e x, e y, and e z are scalars. When a quaternion is used to represent a rotation, we require that it be a unit quaternion, or in other words, e = 1. It is common to refer e 0 as the scalar part of the unit quaternion and the vector defined by e = (e x, e y, e z ) as the vector part. The unit quaternion can be interpreted as a single rotation about an axis in three-dimensional space. For a rotation through the angle Θ about the axis specified by the unit vector v, the scalar part of the unit quaternion is related to the magnitude of the rotation by ( ) Θ e 0 = cos. 2 The vector part of the unit quaternion is related to the axis of rotation by ( ) Θ v sin = e x e y. 2 With this brief description of the quaternion, we can see how the attitude of a MAV can be represented with a unit quaternion. The rotation from the inertial frame to the body frame is simply specified as a single rotation about a specified axis, instead of a sequence of three rotations as required by the Euler angle representation. e z

30 24 QUATERNIONS Figure A.1: Rotation represented by a unit quaternion. The aircraft on the left is shown with the body axes aligned with the inertial frame axes. The aircraft on the left has been rotated about the vector v by Θ = 86 deg. This particular rotation corresponds to the Euler sequence ψ = 90 deg, θ = 15 deg, φ = 30 deg. A.4 CONVERSION BETWEEN EULER ANGLES AND QUATERNIONS There are simple formulas for converting Euler angles to quaternions, and a quaternion to the associated Euler angles. Suppose that the quaternion representing a rotation from the body axes to inertial axes is given by e i b = (e 0, e x, e y, e z ), then the associated Euler angles are φ = atan2 ( 2(e 0 e x + e y e z ), (e e 2 z e 2 x e 2 y) ) θ = asin (2(e 0 e y e x e z )) ψ = atan2 ( 2(e 0 e z + e x e y ), (e e 2 x e 2 y e 2 z) ), where atan2(y, x) is the two-argument arctangent operator that returns the arctangent of y/x in the range [ π, π] using the signs of both arguments to determine the quadrant of the return value. Only a single argument is required for the asin operator since the pitch angle is only defined in the range [π/2, π/2]. Alternatively, given the Euler angles (ψ, φ, θ), the corresponding quaternion representing a passive rotation from the body axes to the inertial

31 Quaternions 25 axes is given by e i b = e 0 e x e y e z cos ψ 2 cos θ 2 cos φ 2 + sin ψ 2 sin θ 2 sin φ 2 = cos ψ 2 cos θ 2 sin φ 2 sin ψ 2 sin θ 2 cos φ 2 cos ψ 2 sin θ 2 cos φ 2 + sin ψ 2 cos θ 2 sin φ 2. sin ψ 2 cos θ 2 cos φ 2 cos ψ 2 sin θ 2 sin φ 2 A.5 CONVERSION BETWEEN QUATERNIONS AND ROTATION MATRICES Given a quaternion e i b that represents a passive rotation from the body axes to the inertial axes, the associated rotation matrix is given by e 2 Rb i = 0 + e2 x e 2 y e 2 z 2(e x e y e 0 e z ) 2(e x e z + e 0 e y ) 2(e x e y + e 0 e z ) e 2 0 e2 x + e 2 y e 2 z 2(e y e z e 0 e x ). 2(e x e z e 0 e y ) 2(e y e z + e 0 e x ) e 2 0 e2 x e 2 y + e 2 z Alternatively, suppose that Rb i = r 11 r 12 r 13 r 21 r 22 r 23, r 31 r 32 r 33 then the associated quaternion e i b = (e 0, e x, e y, e z ) can be computed as follows [4]: e 0 = e x = e y = e z = { (r12 r 21 ) 2 +(r 13 r 31 ) 2 +(r 23 r 32 ) 2 2 { (r12 +r 21 ) 2 +(r 13 +r 31 ) 2 +(r 23 r 32 ) 2 2 { (r12 +r 21 ) 2 +(r 13 r 31 ) 2 +(r 23 +r 32 ) 2 2 { r11 + r 22 + r 33, if r 11 + r 22 + r 33 > 0 3 r 11 r 22 r 33, otherwise 1 + r11 r 22 r 33, if r 11 r 22 r 33 > 0 3 r 11 +r 22 +r 33, otherwise 1 r11 + r 22 r 33, if r 11 + r 22 r 33 > 0 3+r 11 r 22 +r 33, otherwise 1 r11 r 22 + r 33, if r 11 r 22 + r 33 > 0 (r12 r 21 ) 2 +(r 13 +r 31 ) 2 +(r 23 +r 32 ). 2 3+r 11 +r 22 r 33, otherwise

32 26 QUATERNIONS A.6 QUATERNION KINEMATICS Suppose that q represents a small rotation, i.e., Then Therefore cos( Θ/2) q = sin( Θ/2)u x sin( Θ/2)u y sin( Θ/2)u z e(t + t) = M(q )e(t). e(t + t) e(t) ė = lim t 0 t M(q )e(t) e(t) = lim t 0 t = lim t 0 = lim t 0 = 1 2 Ω(ω)e(t), 1 ω x t 2 ω y t 2 ω z t 2. (M(q ) I)e(t) t 0 ω x t ω y t ω z t 1 ω x t 0 ω z t ω y t 2 t ω y t ω z t 0 ω x t e(t) ω z t ω y t ω x t 0 where 0 ω x ω y ω z Ω(ω) = ω x 0 ω z ω y ω y ω z 0 ω x ω z ω y ω x 0 A.7 AIRCRAFT KINEMATIC AND DYNAMIC EQUATIONS Using a unit quaternion to represent the aircraft attitude, Equations (??) through (??), which describe the MAV kinematics and dynamics, can be

33 Quaternions 27 reformulated as e 2 x + e 2 0 e2 y e 2 z 2(e x e y e z e 0 ) 2(e x e z + e y e 0 ) ṗn ṗ e = 2(e x e y + e z e 0 ) e 2 y + e 2 0 e2 x e 2 z 2(e y e z e x e 0 ) u v ṗ d 2(e x e z e y e 0 ) 2(e y e z + e x e 0 ) e 2 z + e 2 0 e2 x e 2 y w u v rv qw = pw ru + 1 f x f y, ẇ qu pv m f z ė 0 0 p q r e 0 ė x ė y = 1 p 0 r q e x 2 q r 0 p e y ė z r q p 0 e z ṗ q = Γ 1pq Γ 2 qr Γ 3 l + Γ 4 n Γ 5 pr Γ 6 (p 2 r 2 ) + 1 J y m. ṙ Γ 7 pq Γ 1 qr Γ 4 l + Γ 8 n (A.1) (A.2) (A.3) (A.4) Note that the dynamic equations given by Equations (A.2) and (A.4) are unchanged from Equations (??) and (??) presented in the summary of Chapter 3. However, care must be taken when propagating Equation (A.3) to ensure that e remains a unit quaternion. If the dynamics are implemented using a Simulink s-function, then one possibility for maintaining e = 1 is to modify Equation (A.3) so that in addition to the normal dynamics, there is also a term that seeks to minimize the cost function J = 1 8 (1 e 2 ) 2. Since J is quadratic, we can use gradient descent to minimize J, and Equation (A.3) becomes ė 0 0 p q r e 0 ė 1 ė 2 = 1 p 0 r q e x 2 q r 0 p e y λ J e ė 3 r q p 0 e z = 1 2 λ(1 e 2 ) p q r p λ(1 e 2 ) r q q r λ(1 e 2 ) p r q p λ(1 e 2 ) where λ > 0 is a positive gain that specifies the strength of the gradient descent. In our experience, a value of λ = 1000 seems to work well, but in Simulink, a stiff solver like ODE15s must be used. This method for maintaining the orthogonality of the quaternion during integration is called Corbett-Wright orthogonality control and was first introduced in the 1950 s for use with analog computers [3,?]. e 0 e x e y e z,

34 uavbook supplement January 22, 2019

35 uavbook supplement January 22, 2019 Appendix B Simulation Using Object Oriented Programming B.1 INTRODUCTION Object oriented programming is well adapted to the implementation of complex dynamic systems like the simulator project developed in the book. This supplement will explain how the Python programming language can be used to implement the project. We will outline one particular way that the simulator can be constructed, as well as the specific data structures (messages) that are passed between elements of the architecture. The architecture outlined in the book is shown for convenience in Figure B.1. The basic idea beh destination, obstacles waypoints path definition airspeed, altitude, heading, commands path planner path manager path following autopilot map status tracking error position error servo commands wind state estimator unmanned aircraft on-board sensors Figure B.1: Architecture outlined in the uavbook. ˆx(t) B.2 NUMERICAL SOLUTIONS TO DIFFERENTIAL EQUATIONS This section provides a brief overview of techniques for numerically solving ordinary differential equations, when the initial value is specified. In particular, we are interested in solving the differential equation dx (t) = f(x(t), u(t)) dt (B.1)

36 30 SIMULATION USING OBJECT ORIENTED PROGRAMMING with initial condition x(t 0 ) = x 0, where x(t) R n, and u(t) R m, and where we assume that f(x, u) is sufficiently smooth to ensure that unique solutions to the differential equation exist for every x 0. Integrating both sides of Equation (B.1) from t 0 to t gives x(t) = x(t 0 ) + t t 0 f(x(τ), u(τ)) dτ. (B.2) The difficulty with solving Equation (B.2) is that x(t) appears on both sides of the equation, and cannot therefore be solved explicitly for x(t). Numerical solutions techniques for ordinary differential equations attempt to approximate the solution by approximating the integral in Equation (B.2). Most techniques break the solution into small time increments, usually equal to the sample rate, which we denote by T s. Accordingly we write Equation (B.2) as x(t) = x(t 0 ) + t Ts = x(t T s ) + t 0 f(x(τ), u(τ)) dτ + t t T s f(x(τ), x(τ)) dτ. t t T s f(x(τ), u(τ)) dτ (B.3) The most simple form of numerical approximation for the integral in Equation (B.3) is to use the left endpoint method as shown in Figure B.2, where t t T s f(x(τ), u(τ)) dτ T s f(x(t T s ), u(t T s )). Figure B.2: Approximation of integral using the left endpoint method results in Runge-Kutta first order method (RK1).

37 Simulation Using Object Oriented Programming 31 Using the notation x k = x(t0 +kt s ) we get the numerical approximation to the differential equation in Equation (B.1) as x k = x k 1 + T s f(x k 1, u k 1 ), x 0 = x(t 0 ). (B.4) Equation (B.4) is called the Euler method, or the Runge-Kutta first order method (RK1). While the RK1 method is often effective for numerically solving ODEs, it typically requires a small sample size T s. The advantage of the RK1 method is that it only requires one evaluation of f(, ). To improve the numerical accuracy of the solution, a second order method can be derived by approximating the integral in Equation (B.3) using the trapezoidal rule shown in Figure??, where Accordingly, b a ( ) f(a) + f(b) f(ξ)dξ (b a). 2 Figure B.3: Approximation of integral using the trapezoidal rule results in Runge- Kutta second order method (RK2). t f(x(τ), u(τ))dτ T s t T s 2 [ f(x(t Ts ), u(t T s )) + f(x(t), u(t)) ]. Since x(t) is unknown, we use the RK1 method to approximate it as x(t) x(t T s ) + T s f(x(t T s ), u(t T s )). In addition, we typically assume that u(t) is constant over the interval [t

38 32 SIMULATION USING OBJECT ORIENTED PROGRAMMING T s, T s ) to get t f(x(τ), u(τ))dτ T s t T s 2 [ f(x(t T s ), u(t T s )) ] + f(x(t T s ) + T s f(x(t T s ), u(t T s )), u(t T s )). Accordingly, the numerical integration routine can be written as x 0 = x(t 0 ) X 1 = f(x k 1, u k 1 ) X 2 = f(x k 1 + T s X 1, u k 1 ) (B.5) x k = x k 1 + T s 2 (X 1 + X 2 ). Equations (B.5) is the Runge-Kutta second order method or RK2. It requires two function calls to f(, ) for every time sample. The most common numerical method for solving ODEs is the Runge- Kutta forth order method RK4. The RK4 method is derived using Simpson s Rule b a ( b a f(ξ)dξ 6 ) ( f(a) + 4f ( a + b 2 ) ) + f(b), which is found by approximating the area under the integral using a parabola, as shown in Figure??. The standard strategy is to write the approximation Figure B.4: Approximation of integral using Simpson s rule results in Runge-Kutta fourth order method (RK4). as b ( ) ( b a f(ξ)ξ f(a) + 2f 6 a ( a + b 2 ) + 2f ( a + b 2 ) ) + f(b),

39 Simulation Using Object Oriented Programming 33 and then to use X 1 = f(a) X 2 = f(a + T s 2 X 1) f X 3 = f(a + T s 2 X 2) f X 4 = f(a + T s X 3 ) f(b), ( ) a + b 2 ( ) a + b 2 (B.6) (B.7) (B.8) (B.9) resulting in the RK4 numerical integration rule x 0 = x(t 0 ) X 1 = f(x k 1, u k 1 ) X 2 = f(x k 1 + T s 2 X 1, u k 1 ) (B.10) X 3 = f(x k 1 + T s 2 X 2, u k 1 ) X 4 = f(x k 1 + T s X 3, u k 1 ) x k = x k 1 + T s 6 (X 1 + 2X 2 + 2X 3 + X 4 ). It can be shown that the RK4 method matches the Taylor series approximation of the integral in Equation (B.3) up to the fourth order term. Higher order methods can be derived, but generally do not result in significantly better numerical approximations to the ODE. The step size T s must still be chosen to be sufficiently small to result in good solutions. It is also possible to develop adaptive step size solvers. For example the ODE45 algorithm in Matlab, solves the ODE using both an RK4 and an RK5 algorithm. The two solutions are then compared and if they are sufficiently different, then the step size is reduced. If the two solutions are close, then the step size can be increased. A Python implementation of the RK4 method for the dynamics ) (ẋ1 = ẋ 2 ( y = x 1 x 2 sin(x 1 ) + u ), with initial conditions x = (0, 0), would be as follows: import numpy a s np import m a t p l o t l i b. p y p l o t as p l t c l a s s dynamics :

40 34 SIMULATION USING OBJECT ORIENTED PROGRAMMING def i n i t ( s e l f, Ts ) : s e l f. t s = Ts # s e t i n i t i a l c o n d i t i o n s s e l f. s t a t e = np. a r r a y ( [ [ 0. 0 ], [ 0. 0 ] ] ) def u p d a t e ( s e l f, u ) : I n t e g r a t e ODE u s i n g Runge K u t t a RK4 a l g o r i t h m t s = s e l f. t s X1 = s e l f. d e r i v a t i v e s ( s e l f. s t a t e, u ) X2 = s e l f. d e r i v a t i v e s ( s e l f. s t a t e + t s / 2 X1, u ) X3 = s e l f. d e r i v a t i v e s ( s e l f. s t a t e + t s / 2 X2, u ) X4 = s e l f. d e r i v a t i v e s ( s e l f. s t a t e + t s X3, u ) s e l f. s t a t e += t s / 6 ( X1 + 2 X2 + 2 X3 + X4 ) def o u t p u t ( s e l f ) : R e t u r n s s y s t e m o u t p u t y = s e l f. s t a t e. item ( 0 ) return y def d e r i v a t i v e s ( s e l f, x, u ) : R e t u r n x d o t f o r t h e dynamics x d o t = f ( x, u ) x1 = x. item ( 0 ) x2 = x. item ( 1 ) x 1 d o t = x2 x 2 d o t = np. s i n ( x1 ) + u x d o t = np. a r r a y ( [ [ x 1 d o t ], [ x 2 d o t ] ] ) return x d o t # i n i t i a l i z e t h e s y s t e m Ts = # s i m u l a t i o n s t e p s i z e system = dynamics ( Ts ) # i n i t i a l i z e t h e s i m u l a t i o n t i m e and p l o t data s i m t i m e = 0. 0 time = [ s i m t i m e ] y = [ system. o u t p u t ( ) ] # main s i m u l a t i o n loop while s i m t i m e < : u=np. s i n ( s i m t i m e ) # s e t i n p u t t o u=s i n ( t ) system. u p d a t e ( u ) # p r o p a g a t e t h e s y s t e m dynamics s i m t i m e += Ts # i n c r e m e n t t h e s i m u l a t i o n t i m e # u p d a t e d ata f o r p l o t t i n g time. append ( s i m t i m e ) y. append ( system. o u t p u t ( ) ) # p l o t o u t p u t y p l t. p l o t ( time, y )

41 Simulation Using Object Oriented Programming 35 p l t. x l a b e l ( time ( s ) ) p l t. y l a b e l ( o u t p u t y ) p l t. show ( )

42 uavbook supplement January 22, 2019

43 uavbook supplement January 22, 2019 Appendix C Animation In the study of aircraft dynamics and control, it is essential to be able to visualize the motion of the airframe. In this appendix we describe how to create 3D animation using Python, Matlab, and Simulink. C.1 3D ANIMATION USING PYTHON C.2 3D ANIMATION USING MATLAB C.3 3D ANIMATION USING SIMULINK The stick-figure drawing shown in Figure?? can be improved visually by using the vertex-face structure in Matlab. Instead of using the plot3 command to draw a continuous line, we will use the patch command to draw faces defined by vertices and colors. The vertices, faces, and colors for the spacecraft are defined in the Matlab code listed below. f u n c t i o n [V, F, p a t c h c o l o r s ]= s p a c e c r a f t % D e f i n e t h e v e r t i c e s ( p h y s i c a l l o c a t i o n o f v e r t i c e s ) V = [ ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t ;... % p o i n t 12 ] ; % d e f i n e f a c e s as a l i s t o f v e r t i c e s numbered above F = [... 1, 2, 6, 5 ;... % f r o n t 4, 3, 7, 8 ;... % back 1, 5, 8, 4 ;... % r i g h t

44 38 ANIMATION 2, 6, 7, 3 ;... % l e f t 5, 6, 7, 8 ;... % t o p 9, 10, 11, 1 2 ;... % bottom ] ; % d e f i n e c o l o r s f o r each f a c e myred = [ 1, 0, 0 ] ; mygreen = [ 0, 1, 0 ] ; myblue = [ 0, 0, 1 ] ; myyellow = [ 1, 1, 0 ] ; mycyan = [ 0, 1, 1 ] ; p a t c h c o l o r s = [... myred ;... % f r o n t mygreen ;... % back myblue ;... % r i g h t myyellow ;... % l e f t mycyan ;... % t o p ] ; end mycyan ;... % bottom The vertices are shown in Figure?? and are defined in Lines The faces are defined by listing the indices of the points that define the face. For example, the front face, defined in Line 19, consists of points Faces can be defined by N-points, where the matrix that defines the faces has N columns, and the number of rows is the number of faces. The color for each face is defined in Lines Matlab code that draws the spacecraft body is listed below. f u n c t i o n h a n d l e = d r a w S p a c e c r a f t B o d y ( pn, pe, pd, phi, t h e t a, p s i, handle, mode ) % d e f i n e p o i n t s on s p a c e c r a f t [V, F, p a t c h c o l o r s ] = s p a c e c r a f t ; % r o t a t e and t h e n t r a n s l a t e s p a c e c r a f t V = r o t a t e (V, phi, t h e t a, p s i ) ; V = t r a n s l a t e (V, pn, pe, pd ) ; % t r a n s f o r m NED t o ENU f o r r e n d e r i n g R = [... 0, 1, 0 ;... 1, 0, 0 ;... 0, 0, 1 ;... ] ; V = V R ; i f isempty ( h a n d l e ) h a n d l e = patch ( V e r t i c e s, V, Faces, F,... FaceVertexCData, p a t c h c o l o r s,... FaceColor, f l a t,... EraseMode, mode ) ; e l s e

45 Animation 39 s e t ( handle, V e r t i c e s,v, Faces, F ) ; end end The transposes in Lines 3 and 4 are used because the physical positions in the vertices matrix V are along the rows instead of the columns. A rendering of the spacecraft using vertices and faces is given in Figure C.1. Additional Figure C.1: Rendering of the spacecraft using vertices and faces. examples using the vertex-face format can be found at the book website.