Institutionen för systemteknik Department of Electrical Engineering Examensarbete Positioning and Trajectory Control of an Autonomous Helicopter using Real Time Kinematic GPS Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet av Josef Högberg and Staffan Sjöqvist LiTH-ISY-EX--13/4691--SE Linköping 2013 Department of Electrical Engineering Linköpings universitet SE-581 83 Linköping, Sweden Linköpings tekniska högskola Linköpings universitet 581 83 Linköping Positioning and Trajectory Control of an Autonomous Helicopter using Real Time Kinematic GPS Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet av Josef Högberg and Staffan Sjöqvist LiTH-ISY-EX--13/4691--SE Handledare: Patrik Axelsson isy, Linköpings universitet Torkel Danielsson Intuitive Aerial AB Examinator: David Törnqvist isy, Linköpings universitet Linköping, 13 juni 2013 Avdelning, Institution Division, Department Datum Date Division of Automatic Control Department of Electrical Engineering SE-581 83 Linköping 2013-06-13 Språk Language Rapporttyp Report category ISBN 2 Svenska/Swedish 2 Licentiatavhandling ISRN 2 Engelska/English 2 Examensarbete 2 C-uppsats 2 D-uppsats 2 2 Övrig rapport — LiTH-ISY-EX--13/4691--SE Serietitel och serienummer Title of series, numbering ISSN — 2 URL för elektronisk version http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-XXXXX Titel Title Positionering och banföljning av en autonom helikopter med hjälp av Real Time Kinematic GPS Positioning and Trajectory Control of an Autonomous Helicopter using Real Time Kinematic GPS Författare Author Josef Högberg and Staffan Sjöqvist Sammanfattning Abstract Under examensarbetet har reglersystemet för en quadrokopter utvecklats. En quadrokopter är en liten helikopter med fyra rotorer som styrs med hjälp av en handhållen kontroll av en operatör positionerad på marken. Tre olika huvudämnen behandlas i detta examensarbete. Till att börja med utvärderas ett Real Time Kinematic Global Positioning System (RTK-GPS). Sedan definieras en mjuk rörelsebana som farkosten i framtiden autonomt kommer att reglera efter. Till sist behandlas styrsystemet i quadrokoptern för att erhålla goda flygegenskaper i det manuella fallet och exempelvis altitudhållning i det autonoma fallet. Utvärderingen av RTK-GPS-systemet resulterade i att det är möjligt att uppnå centimeternoggrannhet i positionsbestämningen. I rapporten diskuteras också vad RTK-GPSsystemet kräver för att det ska fungera så bra som möjligt. En mjuk rörelsebana kan definieras på olika sätt. Önskemålet är att banan ska ha kontinuerlig första- och andraderivata, egenskaper som kubiska spline-funktioner besitter. Reglerprestandan förbättrades genom att krafter och vinkelhastigheter transformerades mellan de två kordinatsystemen som nyttjades. På detta sätt blev styrningen av quadrokoptern mjukare. Nyckelord Keywords Real Time Kinematic GPS, quadrocopter, motion path, cubic splines, automatic control Sammanfattning Under examensarbetet har reglersystemet för en quadrokopter utvecklats. En quadrokopter är en liten helikopter med fyra rotorer som styrs med hjälp av en handhållen kontroll av en operatör positionerad på marken. Tre olika huvudämnen behandlas i detta examensarbete. Till att börja med utvärderas ett Real Time Kinematic Global Positioning System (RTK-GPS). Sedan definieras en mjuk rörelsebana som farkosten i framtiden autonomt kommer att reglera efter. Till sist behandlas styrsystemet i quadrokoptern för att erhålla goda flygegenskaper i det manuella fallet och exempelvis altitudhållning i det autonoma fallet. Utvärderingen av RTK-GPS-systemet resulterade i att det är möjligt att uppnå centimeternoggrannhet i positionsbestämningen. I rapporten diskuteras också vad RTK-GPS-systemet kräver för att det ska fungera så bra som möjligt. En mjuk rörelsebana kan definieras på olika sätt. Önskemålet är att banan ska ha kontinuerlig första- och andraderivata, egenskaper som kubiska spline-funktioner besitter. Reglerprestandan förbättrades genom att krafter och vinkelhastigheter transformerades mellan de två kordinatsystemen som nyttjades. På detta sätt blev styrningen av quadrokoptern mjukare. iii Abstract During this master’s thesis project a control system for a quadrocopter has been developed. A quadrocopter is a small helicopter with four rotors which is controlled by a hand held controller by an operator positioned on the ground. This master thesis report treats three different main subjects. The first one is about evaluating a Real Time Kinematic Global Positioning System (RTK-GPS). The second one is about defining smooth motion paths which the aircraft autonomously shall be able to fly along. The third one is about improving the control system in the aircraft to obtain better flight performance under manual control and to enable autonomous features such as altitude hold. The result is that it is possible to achieve centimetre accuracy in estimated position with the evaluated RTK-GPS system. It is also discussed how the performance of the RTK-GPS system can be maximized. A smooth path can be defined in different ways. It is desirable that the path has continuous first and second derivatives. Therefore cubic spline curves are used to represent the path. The control system was improved by transforming forces and angular velocities between the two different coordinate systems. This resulted in improved abilities by making the steering and control of the aircraft smoother. v Acknowledgments Under examensarbetet har vi haft god hjälp av vår handledare Patrik Axelsson som bidragit med många goda tips och kommentarer, både när det gäller denna rapport och i reglertekniska frågor. Vi har också haft god hjälp av Mårten Svanfeldt och Torkel Danielsson på Intuitive Aerial AB som hjälpt oss lösa många praktiska problem och kommit med värdefulla idéer och synpunkter. Till sist vill vi tacka vår examinator David Törnqvist som hjälpt oss i reglertekniska frågor. Tack! Linköping, Juni 2013 Josef Högberg och Staffan Sjöqvist vii Contents Notation 1 Introduction 1.1 Background . 1.2 Goals . . . . . 1.3 Previous Work 1.4 Thesis Outline xiii . . . . 1 1 1 2 3 2 Background Mathematics Material 2.1 Matrix and Vector Notation . . . . . . . . . . . . . . . . . . . . . . 2.2 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Earth Fixed Coordinate System . . . . . . . . . . . . . . . . 2.2.2 Body Fixed Coordinate System . . . . . . . . . . . . . . . . 2.3 Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.1 Rotational Kinematics . . . . . . . . . . . . . . . . . . . . . 2.3.2 Transforming Angular Velocity . . . . . . . . . . . . . . . . 2.4 Transformation of Forces . . . . . . . . . . . . . . . . . . . . . . . . 2.4.1 Transformation of Forces from Earth frame to Body Frame 2.4.2 Translate Body Fixed Forces to the Earth Frame . . . . . . . 2.5 Translation from Latitude and Longitude to Earth Frame . . . . . 2.5.1 Translation from Latitude to Distance . . . . . . . . . . . . 2.5.2 Conversion from Longitude to Distance . . . . . . . . . . . 5 5 7 7 7 8 8 9 11 11 11 13 13 15 3 Models 3.1 Quadrocopter Model . . . . . . . . . . . . . . . . . . . . . 3.2 Motion Path - A Smooth Trajectory in 3D . . . . . . . . . . 3.2.1 Properties of Spline Curves . . . . . . . . . . . . . 3.2.2 Definition of Spline Curves . . . . . . . . . . . . . 3.2.3 Finding the Closest Point on A Spline Curve . . . . 3.3 AHRS Algoritm . . . . . . . . . . . . . . . . . . . . . . . . 3.3.1 Pros and Cons with Madgwick’s Orientation Filter 17 17 20 21 21 26 29 31 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS ix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 x CONTENTS 4.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.1 Gyroscopes . . . . . . . . . . . . . . . . . . . 4.1.2 Accelerometers . . . . . . . . . . . . . . . . 4.2 Magnetometer . . . . . . . . . . . . . . . . . . . . . 4.2.1 Hard Iron and Soft Iron Distortion . . . . . 4.2.2 Automatic Hard Iron Calibration Algorithm 4.3 Barometer . . . . . . . . . . . . . . . . . . . . . . . 4.4 Internal GPS . . . . . . . . . . . . . . . . . . . . . . 4.5 Sensors and AHRS Filter Performance . . . . . . . 4.5.1 Yaw Rotation . . . . . . . . . . . . . . . . . . 4.5.2 Roll, Pitch and Yaw Rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 33 33 34 34 34 37 42 42 42 43 5 Evaluation of RTK-GPS System 5.1 Evaluation of Trimble’s RTK-GPS . . . . . 5.1.1 Experience and Usages of RTK-GPS 5.1.2 Tests . . . . . . . . . . . . . . . . . 5.2 Error Detection . . . . . . . . . . . . . . . 5.2.1 Maximum Velocity . . . . . . . . . 5.2.2 Maximum Acceleration . . . . . . . 5.2.3 Extrapolated Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 49 49 50 53 53 55 55 6 Control System 6.1 State Estimation . . . . . . . . . 6.1.1 Extended Kalman Filter 6.2 PID-Controllers . . . . . . . . . 6.2.1 Instability Issues . . . . 6.2.2 Feedback Controller . . 6.3 Controllers . . . . . . . . . . . . 6.3.1 Altitude Hold Controller 6.3.2 Yaw Hold Controller . . 6.3.3 Position Hold Controller 6.3.4 Motion Path Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 57 57 58 59 59 59 60 63 64 64 7 Discussions and Conclusions 7.1 Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1.1 Simulation Environment . . . . . . . . . . . . . . . . 7.1.2 Motion Path . . . . . . . . . . . . . . . . . . . . . . . 7.2 Sensors - IMU, Magnetometer, Barometer and Internal GPS 7.3 Evaluation of RTK-GPS System . . . . . . . . . . . . . . . . 7.3.1 Successful Tests . . . . . . . . . . . . . . . . . . . . . 7.3.2 Glitches . . . . . . . . . . . . . . . . . . . . . . . . . 7.4 Future Development . . . . . . . . . . . . . . . . . . . . . . 7.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.6 Experiences . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 67 67 68 68 69 69 69 70 70 71 A Appendix A.1 Equipment Borrowed from Trimble . . . . . . . . . . . . . . . . . . 75 75 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . CONTENTS xi Bibliography 77 Notation Abbreviations Abbreviation dc esc fc gps ia imu ned pid rtk Definition Direct Current Electronic Speed Controller Flight Controller Global Positioning System Intuitive Aerial Inertial Measurement Unit North East Down Proportional, Integral, Differential (controller) Real Time Kinematic xiii xiv Notation Notations Notation EF Force F expressed in the Earth Coordinate System (Earth frame) Force F expressed in the Body Coordinate System (Body frame) BF " Eξ Definition # Eη x y ξ = E z φ θ Eη = ψ u v υ = B w p q ω = B r R W KP KI KD K e(t) u(t) r(t) y(t) p (t|c; t) f (t) c ti x x̂ State vector Position of the aircraft in the Earth frame Euler angles of the aircraft Velocity of the aircraft in the Body frame Angular velocities of the aircraft Rotation matrix, transforming vectors from Earth frame to Body frame Rotation matrix, transforming angular velocities from Earth frame to Body frame Proportional gain Integral gain Derivative gain Kalman gain Control error Input signal Desired output signal, reference signal Actual output signal Spline segment Spline curve Control point in a spline curve Knot in a spline curve State vector Estimated state vector 1 Introduction This chapter contains the background and goals of this master’s thesis. The thesis was performed at Intuitive Aerial AB. 1.1 Background Intuitive Aerial (IA) is developing a flying camera platform for advanced film cameras. The camera platform consists of a multi-rotor helicopter with 12 rotors and a three-axis stabilized camera gimbal. Currently it is adapted to be operated manually but in the future IA wants it to be able to fly along a path autonomously. There are autopilot systems available that can fly along a path defined by discrete waypoints, but that solution provides too little flexibility and control of the trajectory to be usable in a movie recording. This master’s thesis is about the development of the control system and the positioning methods needed to be able to fly along a smooth motion path with high precision. The aircraft used as a development platform is a quadrocopter, see Figure 1.1, with four rotors that are controlled by a control unit (Flight Controller) which is mounted in the centre of the aircraft. The results of this master thesis will be used for the further development of autonomous unmanned aerial vehicles at Intuitive Aerial. 1.2 Goals The goals of the master’s thesis are the following • Improve the flight performance of the aircraft 1 2 1 Introduction Figure 1.1: The aircraft used during the project was this quadrocopter. • Develop a method to define a smooth motion path • Evaluate the performance of an RTK-GPS system 1.3 Previous Work The office of Flying Machine Arena (FMA) is located at Eidgenössische Technische Hochschule Zürich (ETH Zürich). They describe themselves as A space where flying robots live and learn. Quadrocopters are used in their research in adaption and learning because of the agility, hover ability, mechanical simplicity and robustness of these aircraft. Their research is also about trajectory planning and cooperation between multiple quadrocopters. For example they solve planning problems such as constructing small structures of blocks with the help of each other and with different responsibilities during the construction phase. They use a high-precision motion capture system to keep track of the quadrocopters. Models over the system and other data, such as gyroscopes and accelerometers, are used in a sensor fusion framework to predict the state of the object in the future. For more information see [FMA]. The similarities with FMA:s work and this thesis are that quadrocopters are used and that we are interested in the physical model of them. Both are also interested in solving the path planning issues and the control issue. To our help both have IMU:s with three gyroscopes and three accelerometers but there the similarities ends. They use a high-precision motion capture system which gives them a good overview of where their quadrocopters actually are - they know exactly in which point in space their quadrocopters are. Thus they are limited to a confined volume of approximately 10 × 10 × 10 m3 . Instead of a motion-capture system we use a GPS, a barometer and a magnetometer to get position in space, altitude and heading. The quadrocopter has not been limited to a confined volume indoors during this master’s thesis. 1.4 1.4 Thesis Outline 3 Thesis Outline Chapter 1 gives an introduction to the master’s thesis. Chapter 2 gives the fundamental mathematics that are needed to follow the text. Chapter 3 contains a theoretical background that gives a solid ground for the models and algorithms used in the aircraft. After that the sensors used in the aircraft are introduced in Chapter 4 and their pros and cons are discussed. The background and evaluation of the RTK-GPS system from Trimble are presented in Chapter 5. In Chapter 6 the control systems are presented. Chapter 7 is dedicated to discussions and conclusions. 2 Background Mathematics Material This chapter contains an overview of the basic mathematics that are needed to follow the work in the following chapters. The chapter starts with a section about matrix and vector notation and continues with a section about the coordinate systems used. It ends with some transformation mathematics including rotational matrices and conversions between the different coordinate systems. 2.1 Matrix and Vector Notation Matrices and vectors are written with boldface letters, for example F. Left sub indices, E F, for the geometric vectors mark which coordinate system the vectors are represented in. The elements will differ but the vector is the same independently of which coordinate system it is described in. The left sub index E stands for the Earth fixed frame (Earth frame) and the left sub index B stands for the Body fixed frame (Body frame). An example of the above described notation system is given by E Fx F EF = E y . E Fz (2.1) The state vector is given by " Eξ Eη 5 # (2.2) 6 2 Background Mathematics Material where x y Eξ = z is the position of the body in the Earth frame and φ θ Eη = ψ (2.3) (2.4) are the Euler angles of the body expressed in the Earth frame. The velocity of the aircraft expressed in the Body frame is u v υ = (2.5) . B w The angular velocities of the aircraft in the Body frame are p q Bω = r (2.6) In flight mechanics motion in the B X B Z-plane, u, w and q, are called longitudinal and v, p and r are called laterals. Table 2.1 defines the different units that are used in this report and Table 2.2 Measure Angle Frequency Force Voltage Table 2.1: Units used in the report. Unit Symbol In base units radian rad − Hertz Hz s−1 Newton N m · kg · s−2 Volt V m2 · kg · s−3 · A−1 gives a summary of Equations (2.4), (2.5) and (2.6). Table 2.2 is borrowed from Gustafsson [2012]. Table 2.2: Coordinate notation for the rotational kinematic of a body expressed in its Body fixed coordinate system with respect to the Earth fixed coordinate system. Axis Motion components Rotation Euler angle Angular velocities Longitudinal motion u Roll φ p BX Lateral motion v Pitch θ q BY Longitudinal motion w Yaw ψ r BZ 2.2 7 Coordinate Systems 2.2 Coordinate Systems Two coordinate systems are used in this report. Both are NED-systems where NED is the acronym for North, East, Down. The first is fixed to a reference point on Earth and the other is fixed to the aircraft. In Figure 2.1 the two different coordinate systems are shown. rd wa r o F Roll, φ BX Pitch, θ BO BY Yaw, ψ BZ Body fixed coordinate system EO North EX East EY Down EZ Earth fixed coordinate system Figure 2.1: The Earth fixed coordinate system together with the Body fixed coordinate system. 2.2.1 Earth Fixed Coordinate System The Earth fixed coordinate system has its origin E O at the surface of Earth and near the operational area of the aircraft. It may for example be the start position of the aircraft. The E X-axis points northward and the E Y -axis points eastward. The E X E Y -plane is parallel with an idealized surface segment of the Earth around the chosen origin E O. Vectors expressed in the Earth fixed coordinate system have left sub index E. 2.2.2 Body Fixed Coordinate System The Body fixed coordinate system is fixed to the aircraft and rotates with the aircraft. Its origin E O is fixed to the mass centre of the aircraft. The B X-axis points in the forward direction and the B Y -axis points out to the right of the aircraft. Vectors expressed in the Body fixed coordinate system have left sub index B. 8 2 2.3 Background Mathematics Material Transformations In this section some useful transformations between the Earth fixed and Body fixed coordinate systems are introduced and explained. 2.3.1 Rotational Kinematics A vector represented in one coordinate system can be transformed to another system by multiplying it with a rotation matrix that is defined by the Euler angles. The Euler angles are represented by φ θ (2.7) Eη = ψ A rotation from Earth frame to Body frame is performed in the following order: 1. Rotation ψ around the B Z-axis, see rotation matrix Rz (ψ) in (2.8) 2. Rotation θ around the B Y -axis, see rotation matrix Ry (θ) in (2.9) 3. Rotation φ around the B X-axis, see rotation matrix Rx (φ) in (2.10) The rotation matrix Rz (ψ) is given by cos(ψ) Rz (ψ) = − sin(ψ) 0 the rotation matrix Ry (θ) is given by cos(θ) Ry (θ) = 0 sin(θ) sin(ψ) 0 cos(ψ) 0 , 0 1 (2.8) 0 − sin(θ) 1 0 0 cos(θ) and the rotation matrix Rx (φ) is given by 0 1 Rx (φ) = 0 cos(φ) 0 − sin(φ) 0 sin(φ) . cos(φ) (2.9) (2.10) The above rotation matrices are the standard in flight mechanics, see Nelson [1998]. To get the total rotation matrix R equations (2.8) to (2.10) are multiplied together, from the left, in the order mentioned above. R = Rx (φ)Ry (θ)Rz (ψ) = 0 0 cos(θ) 0 − sin(θ) cos(ψ) sin(ψ) 0 1 0 cos(φ) sin(φ) 0 1 0 − sin(ψ) cos(ψ) 0 = 0 − sin(φ) cos(φ) sin(θ) 0 cos(θ) 0 0 1 cos(θ) cos(ψ) cos(θ) sin(ψ) − cos(φ) sin(ψ) + sin(φ) sin(θ) cos(ψ) cos(φ) cos(ψ) + sin(φ) sin(θ) sin(ψ) sin(φ) sin(ψ) + cos(φ) sin(θ) cos(ψ) − sin(φ) cos(ψ) + cos(φ) sin(θ) sin(ψ) (2.11) − sin(θ) sin(φ) cos(θ) cos(φ) cos(θ) 2.3 9 Transformations The transformation from the Earth fixed coordinate system E X E Y E Z to the Body fixed frame B X B Y B Z is expressed as BF = RE F (2.12) where B F stands for a vector in the Body fixed coordinate system and E F for a vector in the Earth fixed coordinate system. The rotation matrix R in (2.11) is orthogonal which means that its inverse is given by its transpose R−1 = RT , (2.13) hence it is easy to transform a vector in the Earth fixed coordinate system to the Body fixed coordinate system; EF = R−1 B F = RT B F (2.14) by using the transpose of R. 2.3.2 Transforming Angular Velocity The transformation of angular velocities E η̇ in the Earth frame to angular velocities B ν in the Body fixed frame is given by Bν = W E η̇ which can be written as 0 p 1 q 0 cos(φ) = r 0 − sin(φ) − sin(θ) φ̇ cos(θ) sin(φ) θ̇ cos()φ) cos(φ) ψ̇ (2.15) (2.16) The inverse to W is used to transform the body’s angular velocities to inertial angular velocities, 1 sin(φ) tan(θ) cos(φ) tan(θ) p φ̇ 0 cos(φ) − sin(φ) q (2.17) θ̇ = cos(φ) sin(φ) r 0 ψ̇ cos(θ) cos(θ) expressed in the Earth frame. Observe the singularities that appears when θ = ± π2 because the denominator cos (θ) becomes zero. With quaternion representation these singularities can be avoided. Derivation of W This derivation is taken from a lecture with Lars Johansson in Flight Dynamics and is reproduced here. Assume that the angular velocities are non-zero, that is ψ̇ , 0 (2.18) θ̇ , 0 φ̇ , 0 (2.19) (2.20) 10 2 Background Mathematics Material and introduce two help coordinate systems x0 y 0 z 0 and x00 y 00 z 00 where the rotation can take place, see Figure 2.2. The first rotation becomes 0 0 = E W̃ ψ̇ (2.21) x0 y 0 z 0 W̃ ψ̇ = ψ̇ where x0 y 0 z 0 rotates relative to the Earth frame E expressed in x0 y 0 z 0 components. The second rotation is 0 θ̇ = x0 y 0 z 0 W̃ θ̇ (2.22) x00 y 00 z 00 W̃ θ̇ = 0 where x00 y 00 z 00 rotates relative to x0 y 0 z 0 expressed in x00 y 00 z 00 components. The third and last rotation is φ̇ 0 = x00 y 00 z 00 W̃ φ̇ (2.23) B W̃ φ̇ = 0 where B rotates relative to x00 y 00 z 00 expressed in B components. B is the new orientation of the aircraft and E express the original orientation of the aircraft, before the orientation. The aircraft follows all three rotations, (2.21) to (2.23), and gets the angular velocity W̃ = x0 y 0 z 0 W̃ ψ̇ + x00 y 00 z 00 W̃ θ̇ + B W̃ φ̇ . (2.24) To add the components in (2.24) a common coordinate system for the component contribution needs to be chosen. With B as the common coordinate system equations (2.9) to (2.10) can be used to transform equations (2.21) to (2.23) which gives W = Rx (φ) Ry (θ) x0 y 0 z 0 W̃ ψ̇ + Rx (φ) x00 y 00 z 00 W̃ θ̇ + B W̃ φ̇ . (2.25) A summary of these steps is shown in Figure 2.2. The inverse to W can be calculated as long as θ , ±90◦ . Rotation 1 ψ E 0Z z EX x0 Rotation 2 x00 x0 θ EY y0 y 00 y0 z0 z 00 Rotation 3 BX x00 BY y 00 φ z 00 BZ Figure 2.2: The help coordinate systems to transform the angular velocities from the Earth frame to the Body frame. 2.4 11 Transformation of Forces 2.4 Transformation of Forces In this section two examples are given of how the rotation matrix R from equation (2.11) is used. 2.4.1 Transformation of Forces from Earth frame to Body Frame Figure 2.3 shows an aircraft that flies in the positive E Y -direction in the Earth fixed coordinate system. Its goal is to go from point A to point B. To obtain this the gravitational acceleration E ag m/s2 is transformed to the Body fixed coordinate system by using B ag = RE ag . Then the force from the four rotors can be calculated to get enough lift force E Fl and also a force E Fa in the right direction towards B. The vector sum of E Fl and E Fa gives the force B F which is the force needed from the propellers to achieve the right thrust. 2.4.2 Translate Body Fixed Forces to the Earth Frame The thrust force B F can also be translated to the Earth fixed frame by the equation EF = R−1 B F (2.26) then the component of the thrust force in E Z-direction gives the lift force of the aircraft. Example of Rotational Kinematics in 2D Above a general rotation matrix R was derived. In this section a rotation in 2D is given as an example of rotational kinematics. The thrust force B F in Figure 2.3 is transformed to the Earth fixed coordinate system with 0 0 F F sin(θ) (2.27) EF = E a = T −E Fl −FT cos(θ) assuming that the aircraft only moves in the yz-plane. Then the accelerations in y-direction and z-direction are calculated by ! FT sin(θ) E ay m E ay m E Fa = ⇔ = sin(θ) ⇔ θ = arcsin (2.28a) E ay = m m FT FT E az = E ag − E Fl = mE ag − (−FT cos(θ)) m mE az − mE ag B FT m = cos(θ) ⇔ FT = ⇔ m(az − E ag ) cos(θ) (2.28b) 12 2 Background Mathematics Material E Fl BF θ BY A θ E Fa B EY BX d, ar w E ag r Fo BZ in dy bo North EX ns se e East EY EO Down EZ Figure 2.3: A model over the aircraft when moving from point A to point B. which finally are used to find θ according to E ay m E ay cos(θ) ⇔ θ = arcsin m( a − a ) ⇔ sin(θ) = E z E g E az − E ag (2.29a) cos(θ) sin (θ) E ay E ay = ⇔ tan(θ) = ⇔ cos(θ) E az − E ag E az − E ag ! E ay arctan E az − E ag (2.29b) (2.29c) In this 2D-case only one Euler angle is unknown, θ, in the 3D-case all three Euler angles are unknown and the rotation matrix R is needed to make the transformation possible. See Luukkonen [2012] for more information about transformations between different coordinate systems. 2.5 Translation from Latitude and Longitude to Earth Frame 13 Table 2.3: Constants used in the transformation from latitudes and longitudes to meters. Constant Value Meaning requator 6 378 137 [m] Earth radius at the equator rnorth pole 6 356 752 [m] Earth radius at the north pole 1 f [m] Flattening constant 298.257223563 2.5 Translation from Latitude and Longitude to Earth Frame The GPS provides the position in latitude and longitude in the WGS84 datum, see Stanaway [2007]. WGS84 is a global standard reference coordinate frame for the Earth that defines a spheroidal surface for altitude and a gravitational equipotential surface for the sea level. The latitude measures the angle from the equator to north and south, see Figure 2.5. The north pole lies on the geodetic latitude of 90◦ north thus all lines of longitude converge at the north pole and at the south pole. The equator lies on latitude 0◦ and the south pole on −90◦ . The longitude measures the angle from Greenwich in eastward and westward direction. It values goes from −180◦ to 180◦ or equivalent from 180◦ west to 180◦ east. 2.5.1 Translation from Latitude to Distance To translate from latitude to meters an origin is needed, in Figure 2.4 the test area is displayed and the origin is marked with a red cross. The constants used in the translation are collected in Table 2.3. At different latitudes a slice of the Earth, parallel to the equator, will have different radii, see Figure 2.5. At the north and the south pole the slice will only cut out a dot with zero meters in radius. The Earth is not a perfect sphere and it is a bit flattened. From the centre of Earth to the equator the mean radius is 6 378 137 m and from the centre to the north pole the radius is 6 356 752 m. The angle between the current latitude φbody and the latitude of the above chosen origin φorigin is labelled ∆φ, ∆φ = φbody − φorigin . (2.30) The tests were performed in the area nearby latitude Φ origin = 58.3934◦ and longitude Λorigin = 15.5617◦ which expressed in radians are φorigin = 1.0192 rad and λorigin = 0.2716 rad. The distance in meter between the aircraft’s position and the origin, in the north- 14 2 Background Mathematics Material Figure 2.4: The test area is displayed above. Origin for the transformation from latitudes/longitudes to meters is located at the edge on the wall, marked with a red cross. N orthpole 90◦ E Xx f = requator 1 − φorigin π/2 ∆φ r EZ ∆φ φorigin a 0◦ Equator Figure 2.5: The blue dotted circle is on latitude 58◦ . The distance x in E Xdirection is given above. 2.5 15 Translation from Latitude and Longitude to Earth Frame ward direction, is x = requator | ! f 1 − φorigin ∆φ π/2 {z } (2.31) r where the flatting constant f is used to account for the Earth’s different radii at the equator and at the north pole. Furthermore, r gives the Earth’s radius at latitude φorigin and ∆φ is the difference in latitude between the origin and the aircraft. The distance in meters, following the circle path between the two points, is then the product between the Earth radius at the current latitude and the angle difference. EY ∆λ N orthpole r3 λorigin y Greenwich Figure 2.6: The Earth seen from above the north pole. 2.5.2 Conversion from Longitude to Distance Two different methods are used to calculate the orthogonal radius from the rotational axis of the Earth to the origin. The first one uses cosine and the radius at the equator, r1 = cos φorigin · requator (2.32) whereas the second one uses sine and the radius at the north pole, π r2 = sin − φorigin · rnorth pole 2 (2.33) 16 2 Background Mathematics Material to calculate the radius r from the centre of Earth to the origin. The resulting radii from (2.32) and (2.33) are weighted together. The weight for the equator radius was set to 90 − Φ origin wa = (2.34) 90 and the weight for the north pole radius was set to wb = Φ origin (2.35) 90 With the above mentioned origin r2 get higher weight because that radius is calculated with the help of the radius at the north pole, which is closer to the above mentioned origin than the equator. In Figure 2.6 an outline of Earth seen from above the the north pole is shown. The radius r3 is displayed where r3 = wa · r1 + wb · r2 (2.36) and the difference in angle is calculated according to ∆λ = λbody − λorigin (2.37) Finally the distance between the longitude of the origin and the position of the aircraft is calculated as y = r3 · ∆λ (2.38) which gives how far away the aircraft is in the eastward direction from the origin, compare with Figure 2.6. With (2.31) and (2.38) the latitude and longitude values can be translated to meters. 3 Models This chapter describes the dynamic model of the aircraft that has been used during the project. 3.1 Quadrocopter Model In Figure 3.1 the forces from the propellers and the gravitational force are shown. The gravitational acceleration E ag is expressed in the Earth fixed coordinate system and the force B Ftot from the propellers are expressed in the body fixed coordinate system. To understand how the gravitational acceleration acts on the body, in the Body fixed frame, it needs to be transformed into the body fixed coordinate system. This is done by using rotational kinematics from Section 2.3.1. The Euler Angles are represented by φ θ (3.1) Eη = ψ where roll φ, pitch θ and yaw ψ are the rotations around the B X-, B Y - B Z-axis respectively. In a +-configured model forward is along one of the booms which is shown in Figure 3.1. The thrust produced by the propeller is proportional to the square of the angular velocity. The revolutions result in a particular torque with respect to the propeller dimension and shape. The motors are voltage controlled and the control system use this to control the force from the rotors. More expensive electronic speed controllers (ESC) are controlled with respect to the angular velocity. A model for the quadrocopter and the direct current motor is presented below. 17 18 3 B F4 B F1 Models rd wa For Roll, φ BX B F3 EO BO North EX East EY Pitch, θ B F2 Y B Yaw, ψ BZ Body fixed coordinate system E ag Down EZ Earth fixed coordinate system Figure 3.1: Earth fixed coordinate system according to the North, East, Down-model. In the Body fixed coordinate system roll (φ) is around the B X-axis, pitch (θ) around the B Y -axis and yaw (ψ) around the B Z-axis. The forces from the four rotors are summed together, = B F1 + B F2 + B F3 + B F4 B Ftot (3.2) where B Ftot is the total force from the four rotors and B F1...4 are the individual forces from each rotor. The angular acceleration calculated around the B X-axis is B ω̇x = (B F4 − B F2 )l Ix (3.3a) B ω̇y = (B F1 − B F3 )l Iy (3.3b) and around the B Y -axis and finally around the B Z-axis B ω̇z = B L2 + B L4 − B L1 − B L3 Iz (3.3c) where l is the distance from the centre to the motor axis. The parameters B ω̇x,y,z has the SI-units rad/s2 , Ix,y,z has the SI-units kgm2 and B L1...4 has the SI-units Nm, see Table 3.1. Each of the propellers gives a thrust force upward B Fn 2 = kt B ωrotor (3.4) 2 = kd B ωrotor (3.5) and a torque around the motor axis B Ln 3.1 19 Quadrocopter Model Table 3.1: Variables used in the dynamic model for the aircraft. Variable SI-unit Description kgm2 Angular momentum B τ Mi h rad i 2 kgm Moment of inertia around one rotor axis BI M kgm2 b Drag constant h radi rad Angular velocity Bω s i h kgm k Lift constant 2 i h rad kgm Lift force for motor i B fi s2 l Length from the centre of the aircraft to the rotor axis [m] where B ωrotor stands for the angular velocity around the rotor axis for each motor n. Both (3.4) and (3.5) are proportional to the angular velocity squared. The parameter kt has the SI-unit kgm and kd has the SI-unit kgm2 . For an electric motor, the two differential equations V = I R + ke B ω + B L dI dt M B ω̇z = kT I − v B ω − τ (3.6) (3.7) are useful. In (3.6) V stands for the voltage applied to the motor, I the current, R the resistance and L for the inductance of the motor. In (3.7) M stands for the rotor’s moment of inertia and τ is the torque applied to the rotor. The constants ke , kT and v are the motor’s back electro magnetic force constant, torque constant and viscous friction constant, respectively. Usually the dynamics depending on the inductance of the motor are fast enough to be neglected, but the motors still have a certain time constant because of the motor and propeller inertia, see Movellan [2010]. The inertia of the aircraft is assumed to be symmetric, the same assumption as in Luukkonen [2012]. That means that Ixx is equal to Iyy and that the inertia matrix is 0 Ixx 0 0 I 0 , (3.8) yy BI = 0 0 Izz which is a diagonal matrix. The angular momentum B τ Mi around the rotor axis is given by B τ Mi = b B ωi2 + B I M B ω̇i (3.9) where the used variables are explained in Table 3.1. The angular momentum around each axis in the body is labelled τφ , τθ and τψ 20 3 Models and is given by 2 2 lk −B ω2 + B ω4 τφ τ lk −B ω12 + B ω32 Bτ = θ = 4 P τψ τ − B Mi (3.10) i=1 where k is the lift constant. 3.2 Motion Path - A Smooth Trajectory in 3D Here, the term motion path is used to describe a path through 3D space that defines the motion of the aircraft. It also has a time stamp related to different sections of the path. This means that at any given time, the system shall be at a certain point along the path. One way to get a 3D-path is to take an earlier flight. In Figure 3.2 such a path is displayed. But this approach lacks the possibility to evaluate it before flight. Smooth motion paths can also be defined using for example 3D Bezier curves or splines which are time coded, see Lyche and Morken [2008]. A fundamental problem is how the path should be defined and what properties it should have to make it suitable as a camera trajectory. Desirable properties for a motion path are: • Possibility to define smooth paths of any shape • Ability to control the velocity and/or flight time along the path • Intuitive for an operator to define and modify the path • There should be an efficient way of calculating the closest point on the path from the current position and to calculate the tangent vector at any point on the path A smooth path in this sense is a path that does not contain any sudden changes in direction or velocity. There are several ways to define a path through the air: • Discrete waypoints • Straight lines connected by circle segments of constant radii • Spline curves Using discrete waypoints provides a simple and intuitive way of defining the path of the aircraft. Straight lines connected by circle segments provide more flexibility than discrete waypoints as smooth turns can be defined. The first two approaches are not good enough because discrete waypoints results in a discontinuous motion and circle segments with constant radii limits the possibilities in making the path. Spline curves are defined by polynomial interpolation between 3.2 21 Motion Path - A Smooth Trajectory in 3D −45 −50 North [m] −55 −60 −65 −70 −50 −80 20 Down [m] −75 25 30 35 40 East [m] 45 50 55 (a) An example of a path that would be possible to fly after. −60 77 76.5 76 25 30 −70 35 40 45 50 North [m] East [m] (b) Same path but seen from the side. The path does not cross itself, although it is difficult to see that from this view. Figure 3.2: An earlier flight log can be used as parameters for an autonomous flight. a set of control points. Curves of any shape can be constructed by using spline curves. 3.2.1 Properties of Spline Curves A spline curve is a polynomial interpolation of a set of control points. Splines can be defined as linear splines, quadratic splines, cubic splines and so forth. The simplest case, linear splines, connect points with straight lines, like waypoints. A quadratic spline connects the control points with polynomials of degree two, and have a continous first derivative. In the same way a cubic spline have both continuous first and second derivatives. In Figure 3.3 and Figure 3.4 a quadratic and cubic spline, respectively, are shown. 3.2.2 Definition of Spline Curves Splines are defined by interpolation between a set of control points. The simplest form of spline is the linear spline, which connect a set of control points by straight lines. A point p on a linear spline segment is defined by t −t t − t1 c1 + c (3.11) p (t|c1 , c2 ; t1 , t2 ) = 2 t2 − t1 t2 − t1 2 where ci are the control points, ti are the knots, and t ∈ [t1 , t2 ]. The knots ti give the value of the parameter t at the corresponding control points. Equation (3.11) computes a point on the line connecting c1 and c2 . The computation is called a convex combination of points c1 and c2 . Convex combinations have the property of being insensitive to round off errors in computers, see page 3 in Lyche and 22 3 2 C2 Models Control points Interpolated points Spline 1.5 P2 1 y P P1 0.5 C3 0 C1 0 0.5 1 1.5 2 x Figure 3.3: A non-uniform quadratic spline in two dimensions with control points (0, 0), (1, 2), (2, 0.5) and knots (0, 0, 1, 1). The points P 1, P 2 and P are evaluated for t = 0.4. Morken [2008]. Quadratic Spline Curves To compose a quadratic spline curve three control points c1 , c2 and c3 are needed together with four knots t1 , t2 , t3 and t4 where t1 6 t2 6 t3 6 t4 . A quadratic spline can be constructed by forming a convex combination of two linear splines according to t −t p (t|c1 , c2 ; t1 , t3 ) p (t|c1 , c2 , c3 ; t1 , t2 , t3 , t4 ) = 3 t3 − t2 t − t2 + p (t|c2 , c3 ; t2 , t4 ) (3.12) t3 − t2 where t ∈ [t2 , t3 ). The line connecting c1 and c2 is denoted p (t|c1 , c2 ; t1 , t3 ) and the line connecting c2 and c3 is denoted p (t|c2 , c3 ; t2 , t4 ). In the case with n control points (ci )ni=1 and n + 1 knots (ti )n+2 i=2 , where the knots are nondecreasing and the two first and two last knots are t2 = t3 and tn+1 = tn+2 , 3.2 23 Motion Path - A Smooth Trajectory in 3D 2.5 Control points Interpolated points Spline 2 C2 C3 P2 P5 1.5 P y P4 P3 1 P1 0.5 0 C1 0 C4 1 2 x 3 4 Figure 3.4: A non-uniform cubic spline in two dimensions with control points (0, 0), (1, 2), (2, 2), (3, 0) and knots (0, 0, 0, 0, 1, 1, 1). a quadratic spline curve f (t) can be denoted pd+1,d (t) t ∈ [td+1 , td+2 ] (t) p t ∈ [td+2 , td+3 ] d+2,d f (t) = . . . .. . p (t) t ∈ [tn tn+1 ] n,d (3.13) Another way to express f (t) is with a sum of all segments, f (t) = n X pi,d (t) Bi,0 (t) (3.14) i=d+1 where Bi,0 (t) are piecewise constant functions defined by ( 1, ti ≤ t ≤ ti+1 Bi,0 (t) = . 0, otherwise (3.15) The difference between two adjacent values in the knot vector can be seen as the time difference between two control points, although the spline generally does not pass through the control points. Cubic Splines The calculation of points on linear and quadratic splines are easily generalized to higher polynomial degrees by using more averages. Calculating a point on an arbitrary spline segment of polynomial degree d requires n = d + 1 control points 24 3 Models c1 , c2 . . . cd , cd+1 and 2d knots (ti )2d i=1 . A spline segment of degree d is then given as the average of two spline segments of degree d − 1 p t|ci−d , ci−(d−1) , . . . , ci ; ti−(d−1) , . . . , ti , ti+1 . . . ti+(d−1) , ti+d t −t = i+1 p t|ci−d , ci−(d−1) , . . . , ci−1 ; ti−(d−1) , . . . ti+(d−2) , ti+(d−1) ti+1 − ti t − ti + p t|ci−(d−1) , ci−(d−2) , . . . , ci ; ti−(d−2) , . . . , ti , ti+(d−1) , ti+d (3.16) ti+1 − ti where i starts at d + 1 for the first segment of the spline curve. The spline curve of degree d and with n control points (ci )ni=1 and 2d knots (ti )2d i=2 is given by (3.13). The spline segment in equation (3.16) can be shortened to psi,d where i stands for the ingoing control points and knots, d stands for the polynomial degree and s gives the gap between the knots in the computation of the weight (t − ti ) / (ti+s − ti ) which is explained at page 24 in Lyche and Morken [2008]. A cubic spline segment requires four control points (n = 4) c1 , c2 , c3 and c4 and six knots (ti )n+2 i=2 . The cubic spline segment is given by p (t|ci−3 , ci−2 , ci−1 , ci ; ti−2 , ti−1 , ti , ti+1 , ti+2 , ti+3 ) t −t p (t|ci−3 , ci−2 , ci−1 ; ti−2 , ti−1 , ti+1 , ti+2 ) = i+1 ti+1 − ti t − ti + p (t|ci−2 , ci−1 , ci ; ti−1 , ti , ti+2 , ti+3 ) ti+1 − ti (3.17) which is a combination of quadratic splines. Matrix Representation of Cubic Spline In order to simplify the evaluation of points on a spline and enable simple calculation of the derivatives of the spline curve the function can be represented on matrix form. As shown in Lyche and Morken [2008][p. 46], a cubic spline can be written on the form h iT f (t) = R1 (t)R2 (t)R3 (t) cµ−3 cµ−2 cµ−1 cµ , (3.18) where ci = µ − 3, ..., µ are the control points that are active on segment µ and Ri are defined by the knots t i according to t−tµ tµ+1 −t R1 (t) = tµ+1 −tµ tµ+1 −tµ = A1 (T 1 t + B 1 ), (3.19) tµ+1 −t t−tµ−1 t −t 0 µ+1 µ−1 tµ+1 −tµ−1 = A2 (T 2 t + B 2 ), R2 (t) = (3.20) tµ+2 −t t−tµ−1 0 tµ+2 −tµ tµ+2 −tµ tµ+1 −t t−tµ−2 t −t 0 0 t −t µ+1 µ−2 µ+1 µ−2 t −t t−t µ+2 µ−1 0 R3 (t) = 0 = A3 (T 3 t + B 3 ). (3.21) tµ+2 −tµ−1 tµ+2 −tµ−1 tµ+3 −t t−tµ 0 0 tµ+3 −tµ tµ+3 −tµ 3.2 25 Motion Path - A Smooth Trajectory in 3D Ri (t) can be rewritten as the matrix product Ri = Ai (T i t + B i ) (3.22) where 1 tµ+1 − tµ 1 0 tµ+1 −t µ−1 = 1 0 tµ+2 −tµ 1 0 tµ+1 −t µ−2 1 = 0 tµ+2 −tµ−1 0 0 h i = tµ+1 −tµ # " tµ+1 −tµ−1 0 = 0 tµ+2 tµ−1 0 tµ+1 −tµ−2 tµ+2 −tµ−1 = 0 0 0 tµ+3 h i = −1 1 # " −1 1 0 = 0 −1 1 0 0 −1 1 = 0 −1 1 0 0 0 −1 1 A1 = (3.23a) A2 (3.23b) A3 B1 B2 B3 T1 T2 T3 0 0 1 tµ+3 −tµ (3.23c) (3.23d) (3.23e) 0 0 t (3.23f) µ (3.23g) (3.23h) (3.23i) Using the relationship (3.22), The multiplication of the R-matrices can be written as R1 (t)R2 (t)R3 (t) = (A1 (T 1 t + B 1 ))(A2 (T 2 t + B 2 ))(A3 (T 3 t + B 3 )) = (A1 T 1 t + A1 B 1 )(A2 T 2 t + A2 B 2 )(A3 T 3 t + A3 B 3 ) = t 3 A1 T 1 A2 T 2 A3 T 3 +t 2 (A1 T 1 A2 T 2 A3 B 3 + A1 T 1 A2 B 2 A3 T 3 + A1 B 1 A2 T 2 A3 T 3 ) +t(A1 T 1 A2 B 2 A3 B 3 + A1 B 1 A2 T 2 A3 B 3 + A1 B 1 A2 B 2 A3 T 3 ) +(A1 B 1 A2 B 2 A3 B 3 ) h i = t 3 t 2 t 1 R̃ (3.24) where A1 T 1 A2 T 2 A3 T 3 A T A T A B + A T A B A T + A B A T A T 1 1 2 2 3 3 1 1 2 2 3 3 1 1 2 2 3 3 R̃ = A1 T 1 A2 B 2 A3 B 3 + A1 B 1 A2 T 2 A3 B 3 + A1 B 1 A2 B 2 A3 T 3 A1 B 1 A2 B 2 A3 B 3 (3.25a) 26 3 Models This allows to separate the parameter t from the matrix R̃ (called the blend matrix) that depends only on the knots tµ−2 . . . tµ+3 that are active on segment µ. By doing this, the blend matrix can be pre-calculated for every segment and the function value at t can be efficiently calculated as h i h iT f (t) = t 3 t 2 t 1 R̃ cµ−3 cµ−2 cµ−1 cµ (3.26) This also makes it easy to calculate the first and second derivatives (velocity and acceleration) at any point on the curve as h i h iT f 0 (t) = 3t 2 2t 1 0 R̃ cµ−3 cµ−2 cµ−1 cµ (3.27) and h f 00 (t) = 6t 2 0 i h 0 R̃ cµ−3 cµ−2 cµ−1 cµ iT (3.28) respectively by simply differentiating f (t) with respect to t. Continuous Derivatives 3.1 Theorem. Suppose that the number ti+1 occurs m times among the knots (tj )m+d j=i−d with m some integer bounded by 1 ≤ m ≤ d + 1, that is ti ≤ ti+1 = · · · = ti+m ≤ ti+m+1 (3.29) Then the spline function f (t) = pi,d,1 (t) Bi,0 (t) + pi+m,d,1 (t) Bi+m,0 (t) has continuous derivatives up to order d − m at the joint ti+1 . For a more detailed derivation of spline curves see page 27 in Lyche and Morken [2008] where this theorem is taken from. 3.2.3 Finding the Closest Point on A Spline Curve The latest known visited position at the curve gives an allowed section of the curve that it is okay to control towards. Together with the wish to follow the path as good as possible it would be sound to control towards the nearest point on the curve but also weighting in to control towards point p(t). In Wang et al. [2003] the problem of finding the closest point on a spline curve is treated. They suggest a combination of Quadratic Minimization Method and Newton’s Method, see Algorithm 1 and Algorithm 2, respectively. The strength in Wang et al. [2003] is that they use the result of the Quadratic Minimization Method as an initial guess for Newton’s Method. The problem is not convex and there are often local minima. Due to the local minima it is important that the initial guesses are in the right region of the spline curve, otherwise the Quadratic Minimization Method will find a point that is suboptimal. After four iterations with Quadratic Minimization Method the result is often very near the optimal solution but continue searching after the optimal solution with Quadratic Minimization Method will converge at super linear rate. Super linear rate means roughly that it often takes very few iterations before the difference is little enough. On the other hand Newton’s Method converges quadratically, which is faster than 3.2 Motion Path - A Smooth Trajectory in 3D 27 super linear, but can get problems in the beginning if the initial guess is poor. Hence it is a good idea to combine these two methods and utilize the advantages of them. To find the point on the curve that lies nearest the aircraft the square distance between the aircraft’s position pa and an optional point along the spline curve, p(t) is calculated. The squared distance is D(t) = (E x(t) − E xa )2 + (E y(t) − E ya )2 + (E z(t) − E za )2 (3.30) t? The goal is to find the value that minimizes D(t) and hence gives the nearest h iT ? ? ? point p(t ) = E x(t ), E y(t ), E z(t ? ) on the curve. Algorithm 1 Quadratic Minimization Method 1. Initial estimates, guess three values of t ? t̃1 , t̃2 and t̃3 (3.31) 2. Calculate a fourth t value 1 q23 D(t̃1 ) + q31 D(t̃2 ) + q12 D(t̃3 ) , k = 1, 2, 3, . . . 2 t23 D(t̃1 ) + t31 D(t̃2 ) + t12 D(t̃3 ) tij = t̃i − t̃j , i, j ∈ {1, 2, 3} t ?,k = qij = t̃i2 − t̃j2 , i, j ∈ {1, 2, 3} (3.32a) (3.32b) (3.32c) 3. Calculate a quadratic polynomial P (t) that interpolates D(t) at t̃1 , t̃2 and t̃3 and then minimize it P (t) = (t − t̃2 ) (t − t̃3 ) D(t̃1 ) (t̃1 − t̃2 ) (t̃1 − t̃3 ) (t − t̃1 ) (t − t̃3 ) D(t̃2 ) + (t̃2 − t̃1 ) (t̃2 − t̃3 ) (t − t̃1 ) (t − t̃2 ) + D(t̃3 ) (t̃3 − t̃1 ) (t̃3 − t̃2 ) (3.33) 4. Pick the three values from t̃1 , t̃2 , t̃3 and t ?,k that gives the smallest value of P (t) and restart at 3 with these estimates and repeat until the difference between the current calculated P (t) and the one before is small enough. The three initial estimates of t ? in step 1 in Algorithm 1 can be initialized in different ways. One choice is to use the previous visited point on the curve, the point that the aircraft should be at according to its velocity and the point it should be at according to the time. Another way is to utilize that the spline curve is divided into segments and the three initial guesses can be chosen as ti , ti +t2i+1 28 3 Models 1.6 1.4 1.2 y 1 0.8 0.6 P1 P5 0.4 P2 P3 P4 Spline Curve Aircraft Closest Point Iterations 0.2 0 1.2 1.4 1.6 1.8 2 x 2.2 2.4 2.6 Figure 3.5: A spline curve and the iterations of the Quadratic Minimization Method. In this case nine iterations were needed (only the first five and the last one is shown) to find the closest point on the curve. The start guesses of t were 0, 0.5 and 3.75. 1 0.8 0.6 0.4 0.2 P3 P5 P4 03 2 3 P1 2 1 P2 00 1 Figure 3.6: Motion path expressed as a cubic spline. 3.3 29 AHRS Algoritm and ti+1 where i denotes the last segment the aircraft was in and i + 1 the next segment that the aircraft will enter. Figure 3.5 shows a graph over a motion path in the E X E Y -plane and the functionality of the Quadratic Minimization Method. The iterations are marked with red stars, the closest point is marked with a circle and the aircraft’s position is marked with a plus sign. The Quadratic Minimization Method was used to find the closest point on the curve. During this test the t-values were chosen in two different segments of the curve, 0, 0.5 and 3.75. This choice of t-values was made to show the functionality of the algorithm and it took nine iterations to find the closest point on the curve, with respect to the allowed error. With t-values chosen with less difference the iterations were fewer and often the iterated points ended very near each other which resulted in a poor plot for the purpose of illustrating the principal of the algorithm. In Figure 3.6 a graph over a motion path Algorithm 2 Newton’s Method 1. Initial estimate 2. Iteration t ?,m+1 = t ?,m − t ?,0 D 0 t ?,m D 00 (t ?,m ) (3.34a) , m = 0, 1, 2, . . . (3.35a) created with a cubic spline is displayed. The closest point to the aircraft and the iterations toward it are also displayed. Newton’s Method uses the tangent of the curve to find a better value. In Algorithm 2, the notation D 0 and D 00 stand for the derivative and second derivative, respectively, of the squared distance D in (3.30) between the aircraft and a point on the curve. 3.3 AHRS Algoritm AHRS stands for Attitude and Heading Reference System and provides an orientation of the aircraft relative to the direction of gravity and the Earth’s magnetic field. In the orientation filter developed by Madgwick [2010] the tri-axis gyroscopes and accelerometers in the IMU are used together with the tri-axis magnetometer to calculate the orientation of the aircraft. This so called MARG implementation makes it possible to compensate for magnetic distortion and gyroscopic bias drift. MARG stands for Magnetic, Angular Rate and Gravity and is a combination of an IMU and a tri-axis magnetometer. MARG is thus the same as AHRS. The orientation filter uses a quaternion representation to avoid the risk of gimbal lock, and uses an optimized gradient-descent algorithm to compute the direction of the measurement error in the gyroscope data. Madgwick’s orientation filter is computationally cheap and effective at low rates down to 10 Hz. Algorithm 3 gives a brief overview of the algorithm, a thorough derivation can be found in Madgwick [2010]. 30 3 Models Algorithm 3 AHRS Filter 1. Integrate angular velocity measurements If a measurement of the angular velocity in the body frame is expressed on the form h i s ω = 0 ωx ωy ωz , (3.36) the quaternion derivative that represents the same rotation can be calculated as 1 q̇ω,k = q̂k−1 ⊗ s ω , (3.37) 2 where ⊗ denotes quaternion multiplication and q̂k−1 is the previous quaternion estimate. The new estimated quaternion q̂ω,k is then calculated by integrating according to q̂ω,k = q̂ω,k−1 + q̇ω,k ∆t, (3.38) where ∆t is the sampling time. 2. Calculate correction based on vector measurements An estimate of the current quaternion can be calculated using vector measurements such as gravity and magnetic field according to f q̂k−1 , E d, B s = q̂∗k−1 ⊗ E d ⊗ q̂k−1 − B s (3.39) E B ∇f q̂k−1 , d, s q̂∇,k−1 = q̂k−1 − µ (3.40) ||∇f q̂k−1 , E d, B s || where E d is the reference vector in the earth frame and B s is the measurement in the body frame. The function ∇f is the gradient of f with respect to the quaternion. Equations (3.39) and (3.40) describe a gradient descent algorithm that uses the previous quaternion estimate as a starting point and makes one step of length µ in the direction of the gradient. 3. Combine the two estimates The two estimates are then weighed together to give the final estimate according to q̂k = γ q̂∇,k + (1 − γ) q̂ω,k , 0 ≤ γ ≤ 1 (3.41) where γ controls the influence of the vector measurements. 4. Normalize quaternion Only quaternions of magnitude 1 represent orientations, therefore the quaternion need to be renormalized every iteration according to q̂ω,k . (3.42) q̂ω,k = ||q̂ω,k || 3.3 31 AHRS Algoritm 3.3.1 Pros and Cons with Madgwick’s Orientation Filter One advantage of Madgwick’s orientation filter is for example a single adjustable parameter. Other advantages of the orientation filter are: • Good performance at low sampling rates • A single adjustable parameter • Gyroscope bias drift compensation • The use of quaternions avoid the problems with the singularities associated with the Euler angle representation • Simple to implement and tune The quaternions used in the filter need to be transformed to Euler angle representation to be able to interpret the orientation of the aircraft. This is done by ! 2q2 q3 − 2q1 q4 ψ=arctan2 (3.43) 2q12 + 2q22 − 1 θ = − arcsin (2q2 q4 + 2q1 q3 ) (3.44) ! 2q3 q4 − 2q1 q2 (3.45) φ=arctan2 2q12 + 2q42 − 1 where arctan2 y =Principal value arg (x + iy) x (3.46) which can be written as y arctan2 = x arctan arctan y yx yx x>0 +π arctan x − π + π2 − π2 undefined and the results are given in the interval (−π, π]. y > 0, x < 0 y y y y < 0, x < 0 > 0, x = 0 < 0, x = 0 = 0, x = 0 (3.47) 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS This chapter describes the different sensors that are used in the flight controller. Their abilities and limitations are discussed under each section. The aircraft uses gyroscopes, accelerometers and magnetometers to measure its orientation and angular velocity relative to the Earth. None of the sensors alone can provide a complete representation of the attitude of the aircraft as they all suffer from different limitations. 4.1 IMU The IMU consists of gyroscopes and accelerometers which are discussed next. 4.1.1 Gyroscopes The aircraft is equipped with three gyroscopes that each measure the angular velocity along one of the axes in the Body fixed coordinate system. The gyroscope suffers from two main drawbacks, bias and noise. If it were not for these drawbacks, the measured angular velocities alone could be integrated to provide a complete representation of the attitude of the aircraft if only the initial orientation would be known. 4.1.2 Accelerometers The three accelerometers in the IMU are used to measure the direction of the gravitational acceleration, which is assumed to always point down (along the positive z-axis) in the Earth frame. They are mounted in three mutually orthogonal directions to be able to measure in three dimensions. One problem with the accelerometers is that they also measure the linear acceleration of the aircraft and 33 34 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS can therefore not be used alone for attitude determination of a non stationary system. 4.2 Magnetometer A three axis magnetometer is used to provide an absolute reference of heading, something that cannot be achieved with only accelerometers and rate gyroscopes. The magnetometer measures the magnetic field in the Body fixed frame. 4.2.1 Hard Iron and Soft Iron Distortion The magnetic measurements are distorted by magnetic objects in the vicinity of the sensor. The distortion can be divided into two categories, hard iron and soft iron distortion. Hard iron distortion can be described as an offset that is constant in the Body frame and not affected by the orientation of the sensor with respect to the Earth’s magnetic field. The hard iron offset is caused by permanent magnets or electric currents close to the sensor. Soft iron distortion is caused by the interaction between the external magnetic field and magnetic materials in the Body frame. The soft iron offset is therefore dependent on the orientation of the sensor relative to the external magnetic field. Hard iron distortion can be compensated for by subtracting the offset from the measurements directly in the Body frame, while compensating for soft iron effects is more complicated, see Vasconcelos et al. [2011] 4.2.2 Automatic Hard Iron Calibration Algorithm The problem of magnetometer bias (hard iron) is addressed by an automatic algorithm that continuously estimates the bias of all three axes during flight. The algorithm is based on the work by Premerlani [2011]. A measurement of the magnetic field is described by B b − B b0 = RE b (4.1) where B b is the measurement in the Body frame, B b0 is the measurement bias, R is the rotation matrix from the Earth frame to the Body frame and E b is the Earth’s magnetic field. The square of the magnitude of both sides of (4.1) is given by |B b − B b0 |2 = |RE b|2 (4.2) Because taking the squared magnitude of a vector is equivalent to taking the dot product of the vector with itself and because R is a rotation matrix and thus preserves vector magnitudes, (4.2) can be rewritten as (B b − B b0 ) · (B b − B b0 ) = |E b|2 , (4.3) where the left side expands to |B b|2 − 2B b · B b0 + |B b0 |2 = |E b|2 . (4.4) 4.2 35 Magnetometer Taking two different measurements, labeled E b1 and E b2 , inserting them in (4.4) and subtracting two instances of the equation gives |B b1 |2 − |B b2 |2 − 2(B b1 − B b2 ) · B b0 = 0, (4.5) which defines a plane in three dimensions with the vector B b1 − B b2 as the normal. The unknown bias B b0 cannot be explicitly calculated from (4.5), but we know that it is located somewhere in the plane. A reasonable approach to estimate the bias vector is to take a small step in the direction of the normal towards the origin every iteration. If the length of the step is scaled by the difference in length of the two measurements and a configurable gain parameter the bias can be estimated by the recursive formula B b̂0,k = B b̂0,k−1 + KM B b1 − B b2 (|B b1 | − |B b2 |) , |B b1 − B b2 | (4.6) where B b̂0,k is the new estimate, B b̂0,k−1 is the previous estimate and KM is the gain of the algorithm. The factor (|B b1 | − |B b2 |) scales the step size so that it decreases as the bias converges. The estimated biases are then subtracted from every measurement. Tuning the gain is a trade off between the time it takes to converge to the correct bias estimate and sensitivity to noise. The risk of division by zero is handled by checking every iteration that the difference of the vectors and only running the algorithm if it is larger than a certain threshold. This also reduces the sensitivity to noise by only updating the bias if a large enough rotation has occurred between the iterations. Part of the magnetometer offset is caused by magnetic fields induced by currents flowing to the motors of the aircraft. Because of this, the offset has been discovered to differ significantly when the motors are on compared to when they are off. This problem was solved by saving two sets of biases, one for when the motors are off and one for when the motors are on. The ARHS filter uses the correctly compensated magnetometer measurements to converge to the correct heading while the aircraft is still on the ground with motors off, and when the motors are turned on the correct offset for that case is used. In Figure 4.2 the magnetometer’s sensitivity noise level at two different motor speeds is tested. Low speed resulted in less noise and high speed resulted in more noise. Figure 4.3 shows the estimated bias during a test flight when the initial value of the bias was set to zero. Figure 4.1 shows raw and calibrated magnetometer data from a test flight where the aircraft was tilted as much as possible in the roll and pitch axes and rotated a few turns around the yaw axis. This provides more information to the calibration algorithm that estimates the bias because the change of the measured magnetic field is bigger between the samples when the magnetic field is measured from many different orientations. In Figure 4.4 the same data as in Figure 4.1 is plotted versus time. 36 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS (a) Raw magnetometer data. (b) Calibrated magnetometer data. (c) Raw magnetometer data, only xand y-components. (d) Calibrated magnetometer data, only x- and y-components. Figure 4.1: Raw and calibrated magnetometer data. 4.3 37 Barometer 0.6 Magnetic Field [G] 0.4 0.2 0 −0.2 −0.4 BX BY BZ 20 25 30 Time [s] 35 40 Figure 4.2: The magnetometer noise at low and at high motor speed, respectively. 4.3 Barometer The barometer that is used is BMP085 which is a digital pressure sensor made by Bosch. Its pressure range is 3000 − 110000 Pa and its altitude range is +9000 m −500 m above sea level, see Sensortec. Figure 4.5 shows an approximately 13 min long test where the air pressure, sensor temperature and the sensor’s height above its start position were measured. The test was performed indoor and the sensor was fixed at a constant altitude during the entire test. Ideally the altitude should have been 0 m during the entire test. The temperature increased slightly during the test due to the heat from the electronics in the confined space of the flight controller. The red line in the middle of the measured altitude data in the third subplot shows the result of the altitude after that a first order filter has worked on the raw data. The first order filter looks as E An = α E an + (1 − α) E An−1 (4.7) where n is the actual sample and n − 1 the preceding sample. The unfiltred altitude is labelled an and the filtred altitude is labelled An . The parameter α is used to tune the filter, in this case α was choosed to 0.35. On page 403 in Söderkvist [2007] there is an introduction to low pass filters. The digital pressure sensor has four different modes; ultra low power, standard, 38 4 0.3 BX BY BZ 0.25 Estimated Magnetometer bias [G] Sensors - IMU, Magnetometer, Barometer and Internal GPS 0.2 0.15 0.1 0.05 0 −0.05 0 50 100 150 200 250 Time [s] Figure 4.3: Magnetometer bias converging during a test flight with a calibration algorithm gain of 0.02. high resolution and ultra high resolution. To decide which mode gives the best estimated altitude the four different modes were tested. The test was performed indoor under similar conditions. In the performed test the sensor stands still on a table during the entire test. Only the first 60 s after the altitude was calculated for the first time was used to compare the different modes. It is important to isolate every factor that can affect the result to be able to compare the different tests. One factor is how long time a test proceeded - a longer test often leads to a bigger standard deviation caused by the natural drift in the sensor and the change of the environment over time. The temperature changed quite fast because the sensor got hot. The standard deviation was used to compare the four different modes. The results were quite similar for the four different modes but according to Table 4.1 the mode with 2 internal samples was best when consideration was taken both to the time it took to update the measured signal and to the standard deviation of the altitude. The standard deviation for altitude did not change much with more internal samples. In Figure 4.6 the four tests are displayed. The standard deviation for the two measured values pressure and temperature and for the calculated value altitude for the four different modes of the digital pressure sensor are shown in Table 4.1. A low σ is better in this case, that would mean that few values are far away from the mean value, which should be zero in this case because the aircraft was fixed to a constant altitude. The low pass filter variable α was here chosen as α = 0.35 to catch the fast changes and be able to follow these with a control signal. 4.3 Barometer 39 Table 4.1: Standard deviation for pressure, temperature and altitude calculated for the four different modes that the barometer have. According to the data sheet, see the BMP085 data sheet by Sensortec, a low number of internal samples results in lower power consumption and a high number of internal samples results in high resolution. Internal number Standard deviation σT [◦ C] σA [m] of samples σP [Pa] 1 6.3598 3.4932 0.4869 2 4.2577 0.3306 0.3836 4 4.2746 0.2536 0.3867 8 4.0348 0.2609 0.3658 Table 4.2: Variables and constants used in the barometric formula (4.8) Variable Value SI-unit Description P − Pressure at actual time and position [Pa] Pb − Pressure at start position [Pa] h i g0 9.80665 m/s2 Gravitational acceleration ? R 8.31432 [Nm/molK] Universal gas constant for air h − Height above start position [m] h0 0 Height at start position [m] M 0.0289644 Mole mass for air [kg/mol] Tb 293.15 Temperature at start position [K] 40 4 1 BX BY BZ 0.8 Estimated Magnetometer bias [G] Sensors - IMU, Magnetometer, Barometer and Internal GPS 0.6 0.4 0.2 0 −0.2 −0.4 0 20 40 Time [s] 60 80 Figure 4.4: Calibrated magnetometer data. To calculate the altitude from the measured pressure and temperature the barometric formula was used. The barometric formula is P = Pb e −g0 M(h−h0 ) R? Tb (4.8) where the used variables are explained in Table 4.2, see Berberan-Santos et al. [1997]. From (4.8), the height above start position is given by R? Tb ln PP b h = h0 − (4.9) g0 M Figure 4.7 shows a test outdoor where the aircraft initially stands on the field for approximately the first 25 seconds and then hover to 1 m altitude. After that the aircraft rises to approximately 10 m altitude quite slowly. Then slowly down to approximately 1 m altitude. Then a fast climb to 10 m followed by a fast drop down to 1 m altitude. These rises and drops have been isolated to the z-direction. The last rise is a fast one which has an inclination of approximately 45◦ against the E X E Y -plane. Although no ground truth data were available the result in Figure 4.7 is quite satisfying since the altitude measurement caught the actions described below. During the flight a test protocol was followed which for each 10 s had an action. The window of 10 s was not quite long enough for all moments since the time it took to rise and drop was longer than estimated. During the first 25 seconds the aircraft is in a start-up mode when calibrations are done. It is important that the aircraft stands still and remains horizontal 4.3 41 Barometer 1.0037 1.0036 1.0035 1.0034 1.0033 0 Altitude [m] Temperature [C] Pressure [Pa] 5 x 10 38 36 34 32 30 28 0 100 200 300 400 Time [s] 500 600 700 100 200 300 400 Time [s] 500 600 700 100 200 300 400 Time [s] 500 600 700 1 0 −1 0 Figure 4.5: Air pressure, sensor temperature and the unit’s height above the start position is shown in the figure. The body was placed indoor on a table and stood still during the entire test. The low pass filter coefficient α was chosen as 0.35. 42 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS 1 0 −1 0 10 20 30 40 50 60 10 20 30 40 50 60 10 20 30 40 50 60 10 20 30 Time [s] 40 50 60 Altitude [m] 1 0 −1 0 1 0 −1 0 1 0 −1 0 Figure 4.6: 1, 2, 4 respective 8 internal samples in each altitude calculation. during this time. 4.4 Internal GPS The model name of the internal GPS device is LEA-6N, see AG. It is a small device, only a couple of centimetres in width, and much lighter than the RTKGPS-system from Trimble. It is mounted on the flight controller and has a small antenna that is mounted on one of the booms. One drawback with the internal GPS-receiver is its precision which is around 50 centimetres instead of a couple of centimetres. In Figure 4.8 the internal GPS receiver’s performance is shown. 4.5 Sensors and AHRS Filter Performance In this section some performance tests of the sensors and AHRS algorithm are presented. 4.5.1 Yaw Rotation Figure 4.9 shows a plot over an indoor test. This test especially shows the magnetometers contribution to the estimated orientation of the aircraft. The aircraft was rotated only around the E Z-axis in order to test the estimated yaw angle. The test was performed indoors, with the motors turned off, by manually rotating the 4.5 43 Sensors and AHRS Filter Performance Pressure [Pa] 5 x 10 1.007 1.006 10 20 30 40 Time [s] 50 60 10 20 30 40 Time [s] 50 60 10 20 30 40 Time [s] 50 60 22.6 22.5 22.4 0 Altitude [m] Temperature [C] 0 10 5 0 0 Figure 4.7: This shows a test flight outdoor where the aircraft was moved up and down in a specific pattern. The low pass filter variable α was here chosen as α = 0.35 to catch the fast changes and be able to follow these. The low pass filter is shown in green and it eliminates fast changes in the measured altitude. aircraft in approximately 90◦ increments, first clock-wise and then counter-clockwise back to the original orientation. Figure 4.9 clearly shows how the magnetometer measurements are affected by the rotation. 4.5.2 Roll, Pitch and Yaw Rotation During this test rotation and estimation of Euler angles around all axes were performed and the result is shown in Figure 4.10. The test was performed indoor, with the motors turned off, by manually rotating the aircraft 45◦ , in positive direction, around every axes and then back again. 1. The first rotation was performed with a positive 45◦ rotation around the E Y -axis, that is pitch, and then back to the original position 2. The second rotation was performed with a positive 45◦ rotation around the E X-axis, that is roll, and then back to the original position 3. The third rotation was performed with a positive 45◦ rotation around the E Z-axis, that is yaw, and then back to the original position 44 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS −20 −30 North [m] −40 −50 −60 −70 −80 −90 20 30 40 50 60 East [m] 70 80 90 Figure 4.8: The internal GPS in the aircraft has approximately half metre accuracy. Here it was moved along the edge of a pathway and should ideally, especially at the ends, be three overlapping curves instead of three parallel. In Figure 4.11 the same test as the previous one was performed, but with the motors turned on. The reason for this is to see how the induced magnetic field in the wires affect the orientation estimation. 4.5 45 Sensors and AHRS Filter Performance Angular Rate [◦ /s] Gyroscope BX BY BZ 100 0 −100 0 20 40 60 80 100 120 140 100 120 140 100 120 140 100 120 140 Acceleration [g] Accelerometer BX BY BZ 1 0 −1 0 20 40 60 80 Time [s] (a) Gyroscope and accelerometer measurements. Magnetic Field B [G] Magnetometer BX BY BZ 0.2 0 −0.2 0 40 60 80 Orientation 200 Orientation [◦ ] 20 BX BY BZ 0 −200 0 20 40 60 80 Time [s] (b) Magnetometer and estimated orientation with help of the AHRS filter. Figure 4.9: During this test the aircraft was rotated only around the E Z-axis. It was rotated 90◦ clockwise to 90◦ and then an additional 90◦ to 180◦ . After that the aircraft was rotated 90◦ counter clockwise back to 90◦ and then an additional 90◦ back to 0◦ . 46 4 Sensors - IMU, Magnetometer, Barometer and Internal GPS Angular Rate [◦ /s] Gyroscope BX BY BZ 50 0 −50 0 20 40 60 80 60 80 Acceleration [g] Accelerometer BX BY BZ 1 0 −1 0 20 40 Time [s] (a) Gyroscope and accelerometer measurements. Magnetic Field B [G] Magnetometer 0.5 BX BY BZ 0 −0.5 0 20 40 60 80 60 80 Orientation Orientation [◦ ] 100 BX BY BZ 0 −100 0 20 40 Time [s] (b) Magnetometer and estimated orientation with help of the AHRS filter. Figure 4.10: During this test the aircraft was rotated around all Body fixed axes with the motors turned off. 4.5 47 Sensors and AHRS Filter Performance Angular Rate [◦ /s] Gyroscope BX BY BZ 50 0 −50 0 20 40 60 80 60 80 Acceleration [g] Accelerometer BX BY BZ 1 0 −1 0 20 40 Time [s] (a) Gyroscope and accelerometer measurements. Magnetic Field B [G] Magnetometer 0.5 BX BY BZ 0 −0.5 0 20 40 60 80 60 80 Orientation Orientation [◦ ] 100 BX BY BZ 0 −100 0 20 40 Time [s] (b) Magnetometer and estimated orientation with help of the AHRS filter. Figure 4.11: During this test the aircraft was rotated around all Body fixed axes with the motors turned on. 5 Evaluation of RTK-GPS System This chapter deals with Trimble’s RTK-GPS, the tests and evaluation of different placements of the GPS-receiver and antennas. 5.1 Evaluation of Trimble’s RTK-GPS During the first two months of the master’s thesis a RTK-GPS system from Trimble was evaluated. The equipment was a base station unit (BX960) and a rover unit (BD920). One big antenna (Antenna Zephyr Model 2) for the base station and two smaller antennas (Antenna AV34) for the rover and the heading units were used. 5.1.1 Experience and Usages of RTK-GPS This section explains some of the experiences gathered and issues encountered from the evaluation of the GPS system. Below are some of the lessons learned during the evaluation of the RTK-GPS: • The base station should be in an unobstructed environment with a clear view of the sky in every direction • The base station antenna should be on a distance from the ground of at least one to two meters • The base station unit’s antenna should be located under an open sky. Trees above and around the antenna will disturb the precision and can cause multipath issues. Trees or other obstacles above the rover unit antenna can disturb the contact with the GPS satellites which can lead to lost tracking of one or more satellites. When 49 50 5 Evaluation of RTK-GPS System −102.4 −102.5 −102.5 −102.6 −102.52 −102.54 −102.8 North [m] North [m] −102.7 −102.9 −103 −102.56 −102.58 −103.1 −102.6 −103.2 −102.62 −103.3 −103.4 88.1 88.2 88.3 88.4 East [m] 88.5 88.6 88.7 (a) During the first 30 seconds RTKGPS was turned off. 88.15 88.2 East [m] 88.25 (b) With RTK on the quality is very high. During a couple of minutes the drift is less than 10 centimetres, with every value counted! Figure 5.1: This first test shows the improvement with RTK compared with traditional GPS. The GPS-receiver is lying still on the ground during the entire test that lasted 7 minutes and 34 seconds. one or more GPS satellites leave the calculation of the position the precision can be reduced. In some cases the estimated position jumped around a half meter from one sample to the next. This often happened when the rover antenna was under trees. When the base station was close to the trees the tree trunks caused multipath issues which gave bias to the rover’s RTK position. Bias in the measurement domain would be difficult to determine in the position domain at the rover when it is in an open sky environment. Moving the base station antenna away from the trees reduced the problem of suddenly jumping position estimates. 5.1.2 Tests Different tests were made to evaluate Trimble’s RTK-GPS-system with respect to precision, repeatability and performance near buildings and obstacles. In the figures the time is colour coded with blue corresponding to the beginning of the test and red to the end. Still on the Ground In the first test the base station was placed under an open sky and the rover unit with the GPS-receiver on it was placed approximately 70 m away. The rover unit was placed still on one location. In the beginning of the test no RTK was activated and in Figure 5.1 it is possible to see the GPS drift over time. But after approximately 30 seconds RTK is enabled and the precision is very high during the rest of the test which lasted for 7 minutes and 34 seconds. 5.1 51 Evaluation of Trimble’s RTK-GPS 0 −5 −15.06 −10 −15.08 −20 North [m] North [m] −15 −25 −15.1 −15.12 −30 −35 −15.14 −40 −45 −10 −5 0 5 10 East [m] 15 20 (a) An overview over the path. 25 −15.16 22.8 22.82 22.84 East [m] 22.86 (b) Zoom in of one of the stand still spots on the path. Figure 5.2: During this test the GPS-receiver was moved four laps clockwise in the displayed path. At two positions along each lap the GPS-receiver stood still a couple of seconds on exactly the same spot. Repeatability In the second test the GPS-receiver was carried several laps along a path. The test data is shown in Figure 5.2. During each lap around the path the GPS-receiver was placed at two distinct spots, which was well-marked with two circles with a diameter of approximately 15 cm. In Figure 5.2b it is possible to see that the error is less than the marked position where to stop. Our conclusions are that this is good enough to be flying after. During the second test, which tested the repeatability, the base station was located under some trees and very near the path so that the antenna was covered by the trees. In some of the tests this lead to loss of consistency and accuracy. In Figure 5.3 the discontinuities in the path due to multipath issues are shown. Trimble explained that the Base station should be under an open sky environment and around 2 meters above the ground to work well. In this test both of these requirements were unfullfilled. The Base station was placed on a bench of approximately 0.5 meters height above the ground and surrounded by some trees. Near Buildings In the third test the base station was still placed on the bench under the trees as above and the rover unit with the GPS-receiver on it was moved along the wall of a building. The resulting path was not as good as during the repeatability test above which was mostly under open sky. Figure 5.4 shows that the RTK-GPS system performs quite good even when it is near high buildings with complex shapes. 52 5 Evaluation of RTK-GPS System 0 −28 −5 −29 −10 −30 −31 North [m] North [m] −15 −20 −25 −32 −33 −34 −30 −35 −35 −36 −40 −37 −45 −10 −5 0 5 10 East [m] 15 20 6 25 (a) During this test the signal was lost after 1.5 lap and before that the system got problems when the rover passed under the trees and near the Base station. 8 10 East [m] 12 14 (b) Multipath issues under the trees lead to a discontinuous path which would have been very difficult to fly after. Figure 5.3: This test shows that Trimble’s equipment does not always provide satisfactory results. −35 −40 −45 North [m] −50 −55 −60 −65 −70 −75 −80 −50 (a) This map is over the area with the building. The picture is taken from Eniro. −40 −30 −20 East [m] −10 0 (b) As suspected the estimated path was not as good as under the open sky environment. But it was surprising that the path glitches over the open sky part of the path, the red part of the path is between the buildings and not near them. Figure 5.4: Trimble’s RTK-GPS system handled the buildings quite well in this test. 5.2 53 Error Detection Repeatability Under an Open Sky During this fourth test the location was totally under an open sky environment and the path was between two lamp posts and a marking on the ground. The rover unit was moved between these markings and at each corner of the triangle it was stopped and kept still for a couple of seconds. The result is impressive, during the five laps all the measurements data, when stopped at the northernmost lamp post, lies inside a square of 4 · 4 cm2 , see Figure 5.5c. The base station was still on the bench under the trees but this time it worked well. 5.2 Error Detection To become more sure of how the aircraft moves in the air and estimate the accuracy of the GPS-data three suggestions of motion models are given. The purpose of these models are to make the system more robust to incorrect values from the GPS-system. The first one uses only a maximum velocity limit, the second one uses a maximum acceleration limit and the third one uses earlier estimated positions. 5.2.1 Maximum Velocity The maximum velocity is denoted B vmax and the sample time is denoted ∆t, with these variables the maximum distance moved during one sample (E smax ) is calculated as E smax = |B vmax | · ∆t. (5.1) With a maximum velocity of 15 m/s the aircraft can maximally move 0.75 m between each sample when the sample time is 5 ms. This means that if the Euclidean distance between the coordinates of two samples exceeds 0.75 meters the control system shall be limited to its maximal control output. To calculate the Euclidean distance between two points in space the latitude value and longitude value given from the GPS-system is transformed into an Earth fixed coordinate system with help of (2.31) and (2.38). Then the Euclidean distance between the actual location denoted E xτ y (5.2) E ξτ = E τ z E τ and the one before, denoted E ξ τ−1 , is calculated according to ∆E ξ = kE ξ τ − E ξ τ−1 k q = (E xτ − E xτ−1 )2 + (E yτ − E yτ−1 )2 + (E zτ − E zτ−1 )2 (5.3a) Equation (5.3a) makes it possible to determine if the actual RTK-GPS-estimated position is probable or not. If the current estimated position has low probability it will affect the following 3D-path in such a way that the system becomes unsure where it has actually been. The reason for this is that it is difficult to decide if the 54 5 Evaluation of RTK-GPS System −40 −42 −42.5 −43 −50 North [m] North [m] −45 −55 −43.5 −44 −44.5 −60 −45 −65 35 40 45 East [m] 50 55 (a) An overview over the path. 47 48 East [m] 48.5 49 (b) Zoom in of one of the stand still spots on the path. −41.92 −41.93 −41.93 −41.935 −41.94 −41.94 −41.95 −41.945 −41.96 North [m] North [m] 47.5 −41.97 −41.95 −41.955 −41.98 −41.96 −41.99 −42 −41.965 −42.01 −41.97 −42.02 48.28 48.3 48.32 48.34 East [m] 48.36 48.38 (c) Even more zoom than in 5.5b which reveals the high accuracy. Notice the axis values. 48.31 48.32 48.33 48.34 East [m] 48.35 48.36 (d) All the measured data was inside a square which measured 4 × 4 cm2 . Figure 5.5: This test was done under an open sky environment. The base station was still on the bench under the trees but this time it worked well. 5.2 55 Error Detection aircraft has visited a place when it is unsure where it is. 5.2.2 Maximum Acceleration The second motion model uses a maximum acceleration approach. The maximum acceleration is denoted amax and the acceleration is calculated using the two preceding samples. In (5.4) and (5.5) the two latest velocities for the aircraft are calculated sτ − sτ−1 (5.4) B vτ = ∆t sτ−1 − sτ−2 ∆t and the current acceleration is calculated as v − vτ−1 = aτ = τ ∆t −sτ−2 sτ −sτ−1 − sτ−1∆t = ∆t = ∆t s − 2sτ−1 + sτ−2 = τ (∆t)2 B vτ−1 = (5.5) (5.6) The acceleration can be allowed to be, for example, at most 12 m/s2 which is comparable to the gravitation at the Earth’s surface which is approximately 9.82 m/s2 . 5.2.3 Extrapolated Position The third motion model uses the mean value of the two preceding samples to estimate how far away the aircraft should be at sample τ compared with sample τ − 1. The mean velocity is given by v + vτ−2 vmean = τ−1 , (5.7) 2 which gives ∆Xest = vmean · ∆t, (5.8) where ∆Xest is an estimation of the displacement since the last sample. If ∆Xest differs significantly from the displacement given by the positioning system the control system can detect that something went wrong between the two samples. The magnitude of the difference is given by r= k∆Xest − ∆Xk2 . k∆Xest k2 (5.9) If r is larger than a certain threshold, it is interpreted as though the positioning system is providing erroneous values. The threshold for r can be tuned depending on the performance of the positioning system. 6 Control System To control the attitude of the aircraft one PID controller for each of the axes is used. 6.1 State Estimation A state, for example altitude of the aircraft, can be estimated from measurements, for example measured altitude and measured acceleration. This is called state estimation and is done with for example sensor fusion. 6.1.1 Extended Kalman Filter The Extended Kalman Filter (EKF) is used to estimate the state of a nonlinear system. The EKF is explained in detail by Gustafsson [2012]. The nonlinear system with added noise can be written ẋ(t) = f (x(t), u(t)) + v(t) (6.1a) y(t) = h(x(t), u(t)) + e(t) (6.1b) where v(t) and e(t) are process noise and measurement noise with covariance matrices Q and R, respectively. f describes the dynamics of the system and h is the measurement equation that relates the measurements y(t) to the state x(t) and the input u(t). In discrete time the model is x k+1 = f (x k , uk ) + v k y k = h (x k , uk ) + ek (6.2a) (6.2b) where x k+1 is the state vector in the next sample and y k is the current measurement vector. The recursion algorithm for the EKF is divided into three parts, one 57 58 6 Control System initialization part, one measurement update and one time update. The initialization part initializes the estimated state vector x̂0|0 and the covariance matrix P 0|0 for the estimated state vector. The measurement update gives the estimated state vector x̂ k|k evaluated at time k given the measurements up to and including time k and the covariance matrix P k|k . The time update predicts the next value of the state vector x̂ k+1|k and updates the covariance matrix P k+1|k . Algorithm 4 The Extended Kalman Filter (EKF) 1. Initialization x̂0|0 = x0 (6.3a) P 0|0 = P 0 (6.3b) 2. Time update x̂ k|k−1 = f x̂ k−1|k−1 , uk ∂f x̂ k−1|k−1 , uk Fk = ∂x̂ k−1|k−1 (6.4a) (6.4b) P k|k−1 = F k P k−1|k−1 F Tk + Qk 3. Measurement update ∂h x̂ k|k−1 , uk Hk = ∂x̂ k|k−1 −1 K k = P k|k−1 H Tk H k P k|k−1 H Tk + Rk (6.5a) (6.5b) x̂ k|k = x̂ k|k−1 + K k (y k − h(x̂ k|k−1 , uk )) −1 H k P k|k−1 P k|k = P k|k−1 − P k|k−1 H Tk H k P k|k−1 H Tk + Rk 6.2 (6.4c) (6.5c) (6.5d) PID-Controllers A PID controller combines proportional gain, integral gain and derivative gain in one controller. The input signal u (t) is based on the difference between the desired and actual output signal e (t) = r (t) − y (t) according to Zt u (t) = KP e (t) + KI e (τ) dτ + KD d e (t) dt (6.6) 0 where KP , KI and KD are the gains for the proportional, integral and derivative parts. A large value of KP gives a faster response to errors but can lead to instability if it is too large. The integral part KI controls the ability to eliminate constant errors in the output signal. The derivative part KD can increase the stability of the system but differentiating noisy measurements can lead to large variations in the control signal. More information about PID controllers can be found in Glad and Ljung [2006]. 6.3 59 Controllers 6.2.1 Instability Issues The cause of instability in a system due to wrong KP and/or KI values is that the feedback controller trusts old information too much. In dynamic systems it takes a certain time before the input signal has an impact on the output signal. This time delay between a change of input signal to the whole impact in the output signal often leads to overcompensation in the feedback controller. When the effect from the input signal finally appears in the output signal the system may have an even bigger error, but with the opposite sign. This will lead to oscillations with increasing amplitude. 6.2.2 Feedback Controller Figure 6.1 shows a block diagram over a feedback system where G is the transfer function of the system and H is the controller. The feedback system gives the r P e H u G y Figure 6.1: A feedback system. following advantages: • Disturbances that affect the output signal are taken into consideration even though they are not measurable by themselves • The system does not need to be fully known • An unstable system can be stabilized and the disadvantage is: • The system can become unstable with poor feedback In the flight controller on the aircraft the feedback control is used specifically to take care of wind disturbances. 6.3 Controllers In Figure 6.2 the control loop for the aircraft is displayed. The Navigation controller sends out which acceleration the aircraft needs for the different axes. In the Angle controller these accelerations are transformed to pitch, roll, yaw angles and thrust that the aircraft should have to achieve the desired accelerations. Depending on which control system that are active the pitch angle θ is translated to an angular velocity ω or a velocity u which the aircraft tries to achieve. The controller that is used is a PID regulator implemented on differential form ∆u = Kp (ek − ek−1 ) + Ki ek + Kd (ek − 2ek−1 + ek−2 ) uk = uk−1 + ∆u (6.7) (6.8) 60 6 Control System Thrust Ψref + Σ - eΨ PID ωref + eω TransΣ formation - PID τ Mixer u Ψ ω Figure 6.2: The attitude control loop. and it is the basis for the following controllers that are used in the aircraft, see Glad and Ljung [2006]. In (6.8) e stands for the error between measured value and the setpoint value. Kp is the proportional gain, Ki is the integral gain and Kd is the differential gain. The time is noted with k for the current sample and k − 1 for the preceding sample. 6.3.1 Altitude Hold Controller The altitude hold controller controls exclusively the aircraft’s altitude. When it is turned on, from the hand held controller, the aircraft holds the current height and the operator is able to change its position in the horizontal plane with help of pitch and roll. In Figure 6.3 the measurements from a manual flight are shown when the aircraft was hoovering at approximately 3 m altitude during the test. Due to discrete motor input from the hand held controller, which is seen in the figure, the aircraft varied in altitude with approximately ±0.20 m around the chosen hovering height. The barometer measurements show much higher variation though, over ±1.00 m around the hovering height. This behaviour in measured sensor input makes it very hard to regulate the aircraft. One attempt to solve this issue was to use a Kalman Filter. In the Kalman Filter the barometer and acceleration are fused together to get a better estimation of the altitude. The state vector x used in the Kalman Filter is defined as " # z x= (6.9) ż where z is the altitude and ż is the velocity in E Z direction. The constant velocity model in continuous time " # " # " # 0 1 0 0 ẋ(t) = x (t) + a (t) + w (t) (6.10a) 0 0 1 1 h i y (t) = 1 0 x (t) (6.10b) and discrete time # " 2# " 2# Ts Ts 1 Ts 2 x k+1 = xk + ak + 2 wk 0 1 Ts Ts h i y k = 1 0 xk " (6.11a) (6.11b) 6.3 61 Controllers with the sampling time Ts , see Gustafsson [2012]. The Kalman Filter is used on the discrete time model (6.11a) and (6.11b). The result of the Kalman Filter is shown in Figure 6.3 as the blue dotted line, which varies approximately ±0.80 m around the hoovering height. The measured motor input data from Figure 6.3 25 Measured Altitude [m] 20 Motor Input [-] Estimated Altitude [m] Value 15 Estimated Velocity [m/s] 10 5 0 −5 −10 70 0 Time [s] Figure 6.3: Estimated altitude and vertical velocity with Kalman Filter. Between the two peaks the aircraft was hoovering at approximately the same height, approximately ±0.20 m around 3 m altitude. The solid cyan line are the measurements from the barometer which is more than ±1.00 m around the hoovering level. The green dashed line shows the estimated velocity, the black line the motor input and the blue dotted line the estimated altitude. varies with approximately 1 Hz and with the amplitude 0.4 motor input units. The resulting altitude controller follows the measurements but due to the poor measurements the altitude controller becomes poor. With better data, from example RTK-GPS, it would probably work better. Another solution could be to use the motor input in the state vector x because the motor input is quite stable and affects the height directly. It would consequently be a good idea to also estimate it with the Kalman Filter. Then the state vector x becomes z x = ż (6.12) K 62 6 Control System g−z̈ where K = u and umotor is the motor input. K has the unit m/s2 because motor both g and z̈ are expressed in m/s2 while umotor is dimensionless. The non-linear model is x2 ẋ (t) = g − x3 umotor (6.13) 0 " # x1 y (t) = (6.14) −x3 umotor where the measurements are barometer and accelerometer. By discretizing (6.13) and (6.14) with Euler sampling the continuous time model becomes x2,k g − x u x k+1 = x k + Ts (6.15) 3,k motor,k 0 # " 1 0 0 x (6.16) yk = 0 0 −umotor k The result after applying the Extended Kalman Filter on (6.15) and (6.16) is shown in Figure 6.4. The use of EKF improved the altitude estimation, compare Figure 6.3 (KF) with Figure 6.4 (EKF), the variance of the estimates are visibly less with the EKF. 25 Measured Altitude [m] 20 Motor Input [-] Estimated Altitude [m] 15 Estimated Velocity [m/s] Value Estimated K [m/s2 ] 10 5 0 −5 −10 0 10 20 30 40 Time [s] 50 60 70 Figure 6.4: Estimated altitude and vertical velocity with Extended Kalman Filter. The input data is the same as in Figure 6.3. 6.3 63 Controllers A model was also made in Simulink in MatLab to try to catch the dynamic behaviour of the system. In Figure 6.5 this model can be seen. White band limited Process noise Control signal 1 PID(s) Step PID Controller 0.05s+1 Saturation Transfer Fcn -K- a 1 s v Integrator Gain 1 s z Integrator1 Altitude 9.82 Gravity 2 200 Hz sampling Gain1 Velocity estimation delay Measurement noise 50 Hz sampling Barometer delay Barometer noise Figure 6.5: Simulink model over the aircraft and the altitude controller. noise together with a delay were added to the model to try to catch as much as possible of the reality. The experience from this implementation is that you need to know the sensors limits and construct your control system thereafter. 6.3.2 Yaw Hold Controller The yaw hold controller keep the aircraft facing the same direction as it was facing when the operator turned this function on. The operator can change the altitude with the thrust lever on the controller and also pitch and roll of the aircraft. Achieving the Correct Euler Angles Given a force E F in the Earth fixed frame and a desired yaw angle it is desirable to translate this into corresponding Euler angles φ, θ and ψ and a body fixed thrust force B F . This can be done with T = k E F k2 φ = arcsin θ = arctan (6.17) −E F x sin(ψ) + E F y cos(ψ) T ! F cos(ψ) + E x E F y sin(ψ) EFz ! (6.18) (6.19) where 0 0 F = B −T (6.20) 64 6 Control System The assumption that the body only has a force in z-direction is made with respect to that the rotors are aligned with the z-axis. From the rotation matrix R and the relation between Earth fixed and Body fixed forces EF = R−1 B F sin(φ) sin(ψ) + cos(φ) sin(θ) cos(ψ) = −T − sin(φ) cos(ψ) + cos(φ) sin(θ) sin(ψ) cos(φ) cos(θ) (6.21) (6.22) the equations (6.17) to (6.19) can be derived, see Zuo [2009]. In the control loop equations (6.17) to (6.19) are used to translate the calculated force E F to Euler angles. 6.3.3 Position Hold Controller The position hold controller keeps the aircraft hoovering on a specific spot in space. The operator can only change the aircraft’s yaw angle. 6.3.4 Motion Path Controller There are different solutions to how the aircraft can fly along a smooth motion path. One solution is to let the forward direction of the aircraft always point in the direction of the path tangent. This approach is intuitive for many people, the vehicle goes in the direction that is ahead of it. But a quadrocopter, which has been studied during this master’s thesis, is quite symmetric and does not have any clear front from the beginning which opens for other solutions. A second solution is that a certain boom of the aircraft always points against one specific point, for example the pilot. This method makes it easier for some helicopter pilots to keep track of roll and pitch controls when they take over the control of the aircraft after a successful autonomous flight. Follow a Spline Curve To be able to follow a smooth motion path defined by a spline curve it can be good to know the closest point on the curve given the aircraft’s position. The parametric cubic spline is expressed as E x(t) y(t) (6.23) p(t) = E , 0 ≤ t ≤ L E z(t) where E x(t), E y(t) and E z(t) are the components of the point p(t) along the spline curve evaluated at t and expressed in the Earth frame. The aircraft’s position is pa = [E xa , E ya , E za ]T . In general these two positions are not the same. One possibility for the control system is to navigate along the shortest path between the current location pa and the desired location p(t) which results in the direction E x(t) E xa ha = p(t) − pa = E y(t) − E ya (6.24) E z(t) E za 6.3 Controllers 65 where ha denotes the desired heading of the velocity of the aircraft. Due to obstacles in the real world environment this control strategy can lead to catastrophic consequences. It is not desirable to take a shortcut through for example a lamp post or a building. A better control strategy would be to try to follow the motion path, the spline curve, as good as possible and relax the time constraint. 7 Discussions and Conclusions In this chapter the results and conclusions from previous chapters are discussed. This chapter is divided in sections corresponding to the previous chapters and ends with Section 7.6 where the experiences from this work are collected and commented. 7.1 Models The quadrocopter model in Chapter 3.1 has not been used during the project however the flight dynamics in Chapter 2 have been used. Especially the rotation matrices R and W have improved the aircraft’s abilities by making the steering and control of the aircraft smoother. 7.1.1 Simulation Environment One experience from this project is that it would have been much easier to achieve good results faster if a good simulation environment had been available. It would maybe have been possible to build an own simulation environment with the model from Chapter 3.1 and physical data about the studied aircraft. This would probably have taken much time to get it good enough to be useful, maybe it would be enough material to do a master’s thesis about simulating a quadrocopter. At last a simple model for the altitude controller was made in Simulink in MatLab, see Figure 6.5. This made it possible to at least see the relative relationship between the control parameters and how these affected the system. The tests were otherwise performed outdoors and one or two of the control parameters were tuned manually during the flight from the hand held controller. This approach was time consuming but had the advantage that the evaluation of 67 68 7 Discussions and Conclusions the controller was made in the real environment. A recommendation to similar projects is to use a simulation environment where it is easy to change control parameters and evaluate them. 7.1.2 Motion Path The smooth motion path was defined with a cubic spline curve. The property of such spline curves is that they are continuous with respect both to their first and second derivatives. That is the derivative of their position and velocity. A thorough derivation of the making of spline curves is given in Chapter 3.2. From linear spline curves, which are the easiest type, to quadratic curves, which provide continuous velocity, up to cubic spline curves are derived. It is also mentioned how to make spline curves of higher degree. With the introduction of matrix representation of splines it becomes easy to evaluate the spline curves in, for example, MatLab or C. The two algorithms, Quadratic Minimization Method and Newton’s Method make the finding of the closest point, relative to the aircraft’s position, on a segment of the curve fast and computationally efficient. The tangential vector in each point of the spline curve can also be displayed together with the acceleration vector. In a further motion path control system splines will provide smooth curves which are defined by a small amount of control points c and knots t. For each segment of the curve only the actual control points and knots for that segment needs to be known together with the blending matrix R̃. These variables are all that needs to be known to calculate points along a curve segment, which make the spline curve representation computationally cheap. Regardless of the advantages of the spline curves, creating the path can be a bit unintuitive the first times. The spline curve does not, in the representation presented, go through the control points and a change of one control point or knot changes the curve. 7.2 Sensors - IMU, Magnetometer, Barometer and Internal GPS The sensors used during the project had different precision. For example the barometer had quite poor precision, see Figure 4.5, while Trimbles RTK-GPS was on the other end of the scale with centimetre precision, see Figure 5.1a. It is unclear if the barometers poor precision is due to the specific sensor used or if it is a common problem for all barometer sensors. Maybe the tested barometer sensor is as good as a barometer sensor possible can be in the different environments that have been tested. It is difficult to say when no other barometers have been tested and evaluated. One thing for sure is that the measurements from the barometer sensor where very difficult for a PID-control system to handle due to the high level of noise. 7.3 Evaluation of RTK-GPS System 69 Good sensor measurements are fundamental for getting good performance from a control system. One experience from the implementation of the internal GPS is that it is important what data type is used to represent latitude and longitude. In the programming language C it is possible to decide how numbers shall be interpreted. For example int, float and double. One problem that was encountered when implementing the GPS in the Flight Controller code was that the precision was low when float was used for the entire coordinate. This was solved by using two floats, the first one holds the minutes part and the second one the fraction. 7.3 Evaluation of RTK-GPS System In this section the results from the evaluation of the RTK-GPS system are discussed and analysed. 7.3.1 Successful Tests The evaluation of Trimble’s RTK-GPS resulted in plots with very high position accuracy. These position data would be possible to use as input to a control algorithm for autonomous flight. For example see Figure 5.5 which shows the very high accuracy under an open sky environment. Important in this figure is that a particular spot was revisited five times with approximately 90 seconds space in between. With a normal GPS-receiver the change of satellite constellation should have given a much higher uncertainty compared with the case with RTK. An ordinary GPS-system should have had meter accuracy in position compared with the achieved centimetre accuracy with Trimble’s system. Compare for example Figure 4.8 which shows the internal GPS with Figure 5.5a which shows the RTKGPS system. In both these figures the aircraft or test bench with the GPS receiver equipment is hand held and moved along two different paths multiple times. The conclusion of this is that a flying aircraft with Trimble’s RTK-GPS system would be able to return to exactly the same spot in the world time after time and it would rather be the control system that sets the limits for the success of a flight. Another constraint is the flight time which for IA:s big hexacopter (six booms) is approximately five minutes. 7.3.2 Glitches During some of the tests Trimble’s RTK-GPS-system turned off and restarted. And some other times it got discontinuance in the calculated positions - the estimated position jumped approximately one meter between two samples. The time between two samples was five milliseconds. This means that the velocity, for the hand-held GPS-receiver based on these data was around 20 m/s between two samples, which corresponds to 72 km/h, which was not the case during the actual test. This can be detected with help of the discussed error detection in Section 5.2. The best way to avoid glitches is to operate the aircraft under an 70 7 Discussions and Conclusions open sky environment and to place the base station away from trees, buildings and similar obstacles. In Figure 5.3 some glitches are shown. Handle Glitches In a control system glitches can be handled with a dynamic model of the aircraft that says that the aircraft’s maximal velocity is 15 m/s which means that the aircraft can maximally move 0.75 m between two time samples. An even more accurate estimation of the aircraft’s position in the next time sample would be to extrapolate, say, two samples back just to get an approximation of how far the aircraft can reach. A third alternative is to use the maximal acceleration and calculate how far away the aircraft can come. 7.4 Future Development This project has big opportunities to be further developed. The lessons from this project can be used to plan a future project better with more time deposited for hardware issues. It would also be good to find a simulation environment in which the ideas can be tested and future developed. When the motionpath system for the aircraft works it would be a good idea to add a similar system for the camera system steering the gimbal in a specific path coupled to the aircraft’s path. It would also be interesting to study commercialising of the system, how shall the user create the paths for example? 7.5 Results The goals for the master’s thesis were the following • Improve the abilities of the aircraft • Develop a motion path system • Evaluate a RTK-GPS system • Implement more sensors in the aircraft • Define a smooth motion path which was mentioned in Chapter 1.2. Of these goals have four out of five been achieved. The abilities of the aircraft have been improved. At the start of the project the most of the code of the flight controller was implemented but it was not tuned for the aircraft used during the project. Develop a motion path system ended with an altitude holder due to lack of time. It took a lot longer time to tune the parameters than calculated. A better altitude estimation together with a better test environment would have shortened this time. 7.6 Experiences 71 The evaluation of RTK-GPS also took a lot of time. There was a lot of hardware issues during the first weeks. After the hardware issues were solved a lot of test material were collected and analysed. During the project sensor reading for magnetometer and two different barometers have been implemented in the flight controller. This made it possible to get altitude and heading estimates of the aircraft. A smooth motion path has been defined with help of splines. This was implemented in MatLab. The advantages with using splines are that a smooth path is achieved and with splines of dimension three the velocity in each point of the spline is continuous. 7.6 Experiences During this project a lot of lessons have been learned. The most important one is that hardware issues takes a lot more time than planned, at least 5 times the amount of time scheduled. Another lesson is that it is important to have a developed test environment to be able to evaluate control systems faster. It is also good to know the pros and cons of the sensors in an early stage of the project and to know how they can be improved and fused together. Personal experiences during the project are a better understanding of hardware, quadrocopter pilot experience and an improved English and improved knowledge in report writing. Appendix A Appendix A.1 Equipment Borrowed from Trimble Figure A.1 displays the equipment that Trimble lent the project. Figure A.1: Trimble lent this equipment to evaluate. 75 76 A Appendix Bibliography Ublox AG. LEA-6 u-blox 6 GPS Modules Data Sheet. http://www.u-blox. com/en/gps-modules/pvt-modules/lea-6-family.html. Cited on page 42. Mário N. Berberan-Santos, Evgeny N. Bodunov, and Lionello Pogliani. On the Barometric Formula. American Association of Physics Teacheers, 65(5):404– 412, 1997. Cited on page 40. Flying Machine Arena (FMA). Flying Machine Arena Publications. http:// www.flyingmachinearena.org/publications/. Cited on page 2. Torkel Glad and Lennart Ljung. Reglerteknik Grundläggande teori. Studentlitteratur, 2006. Cited on pages 58 and 60. Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur, second edition, 2012. Cited on pages 6, 57, and 61. Lars Johansson. Flygmekanik för Y. Cited on page 9. Teppo Luukkonen. Modelling and Control of Quadcopter. Technical Report Mat2.4108, Aalto University - School of Science, Espoo, August 2012. Cited on pages 12 and 19. Tom Lyche and Knut Morken. Spline Methods Draft. Department of Informatics Centre of Mathematics for Applications University of Oslo, 2008. Cited on pages 20, 21, 24, and 26. Sebastian O.H. Madgwick. An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays. April 2010. Cited on page 29. Javier R. Movellan. DC Motors. March 2010. URL http://mplab.ucsd.edu/ wordpress/tutorials/DC.pdf. Cited on page 19. R. C Nelson. Flight Stability and Automatic Control. McGraw-Hill, second edition, 1998. Cited on page 8. 77 78 Bibliography William Premerlani. Magnetometer Offset Cancellation: Theory and Implementation revisited. https://gentlenav.googlecode.com/files/ MagnetometerOffsetNullingRevisited.pdf, 2011. Cited on page 34. Sune Söderkvist. Från insignal till utsignal. Linköping, Sweden, 2007. Cited on page 37. Tryckeriet Erik Larsson AB, Bosch Sensortec. BMP085 Digital Pressure Sensor Data sheet, 1.2 edition. Cited on pages 37 and 39. Richard Stanaway. GDA94, ITRF, WGS84: What’s the Difference? Working with Dynamic Datums. Spatial Sciences Institute - Biennial Conference, Hobart, May 2007. Cited on page 13. J.F. Vasconcelos, G. Elkaim, C. Silvestre, P. Oliveira, and B. Cardeira. Geometric Approach to Strapdown Magnetometer Calibration in Sensor Frame. Aerospace and Electronic Systems, IEEE Transactions on, 47(2):1293–1306, 2011. doi: 10.1109/TAES.2011.5751259. Cited on page 34. Hongling Wang, Joseph Kearney, and Kendall Atkinson. Robust and Efficient Computation of the Closest Point on a Spline Curve. Curve and Surface Design: Saint-Malo 2002, 2003. Cited on page 26. Z. Zuo. Trajectory Tracking Control Design with Command-Filtered Compensation for a Quadrotor. IET Control Theory and Applications, 4(11):2343–2355, 2009. Cited on page 64. Upphovsrätt Detta dokument hålls tillgängligt på Internet — eller dess framtida ersättare — under 25 år från publiceringsdatum under förutsättning att inga extraordinära omständigheter uppstår. Tillgång till dokumentet innebär tillstånd för var och en att läsa, ladda ner, skriva ut enstaka kopior för enskilt bruk och att använda det oförändrat för ickekommersiell forskning och för undervisning. Överföring av upphovsrätten vid en senare tidpunkt kan inte upphäva detta tillstånd. All annan användning av dokumentet kräver upphovsmannens medgivande. För att garantera äktheten, säkerheten och tillgängligheten finns det lösningar av teknisk och administrativ art. Upphovsmannens ideella rätt innefattar rätt att bli nämnd som upphovsman i den omfattning som god sed kräver vid användning av dokumentet på ovan beskrivna sätt samt skydd mot att dokumentet ändras eller presenteras i sådan form eller i sådant sammanhang som är kränkande för upphovsmannens litterära eller konstnärliga anseende eller egenart. För ytterligare information om Linköping University Electronic Press se förlagets hemsida http://www.ep.liu.se/ Copyright The publishers will keep this document online on the Internet — or its possible replacement — for a period of 25 years from the date of publication barring exceptional circumstances. The online availability of the document implies a permanent permission for anyone to read, to download, to print out single copies for his/her own use and to use it unchanged for any non-commercial research and educational purpose. Subsequent transfers of copyright cannot revoke this permission. All other uses of the document are conditional on the consent of the copyright owner. The publisher has taken technical and administrative measures to assure authenticity, security and accessibility. According to intellectual property law the author has the right to be mentioned when his/her work is accessed as described above and to be protected against infringement. For additional information about the Linköping University Electronic Press and its procedures for publication and for assurance of document integrity, please refer to its www home page: http://www.ep.liu.se/ © Josef Högberg and Staffan Sjöqvist

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement