Optimal path planning and control of quadrotor unmanned aerial vehicle for area coverage

Transcription

1 The University of Toledo The University of Toledo Digital Repository Theses and Dissertations 2014 Optimal path planning and control of quadrotor unmanned aerial vehicle for area coverage Jiankun Fan University of Toledo Follow this and additional works at: Recommended Citation Fan, Jiankun, "Optimal path planning and control of quadrotor unmanned aerial vehicle for area coverage" (2014). Theses and Dissertations This Thesis is brought to you for free and open access by The University of Toledo Digital Repository. It has been accepted for inclusion in Theses and Dissertations by an authorized administrator of The University of Toledo Digital Repository. For more information, please see the repository's About page.

2 A Thesis entitled Optimal Path Planning and Control of Quadrotor Unmanned Aerial Vehicle for Area Coverage by Jiankun Fan Submitted to the Graduate Faculty as partial fulfillment of the requirements for the Master of Science Degree in Mechanical Engineering Dr. Manish Kumar, Committee Chair Dr. Mohammad Elahinia, Committee Member Dr. Mehdi Pourazady, Committee Member Dr. Patricia Komuniecki, Dean College of Graduate Studies The University of Toledo December 2014

3 Copyright 2014, Jiankun Fan This document is copyrighted material. Under copyright law, no parts of this document may be reproduced without the expressed permission of the author

4 An Abstract of Optimal Path Planning and Control of Quadrotor Unmanned Aerial Vehicle for Area Coverage By Jiankun Fan Submitted to the Graduate Faculty as partial fulfillment of the requirements for the Master of Science Degree in Mechanical Engineering The University of Toledo December 2014 An Unmanned Aerial Vehicle (UAV) is an aircraft without a human pilot on board. Its flight is controlled either autonomously by computers onboard the vehicle, or remotely by a pilot on the ground, or by another vehicle. In recent years, UAVs have been used more commonly than prior years. The example includes areo-camera where a high speed camera was attached to a UAV which can be used as an airborne camera to obtain aerial video. It also could be used for detecting events on ground for tasks such as surveillance and monitoring which is a common task during wars. Similarly UAVs can be used for relaying communication signal during scenarios when regular communication infrastructure is destroyed. The objective of this thesis is motivated from such civilian operations such as search and rescue or wildfire detection and monitoring. One scenario is that of search and rescue where UAV s objective is to geo-locate a person in a given area. The task is carried out with the help of a camera whose live feed is provided to search and rescue personnel. For this objective, the UAV needs to carry out iii

5 scanning of the entire area in the shortest time. The aim of this thesis to develop algorithms to enable a UAV to scan an area in optimal time, a problem referred to as Coverage Control in literature. The thesis focuses on a special kind of UAVs called quadrotor that is propelled with the help of four rotors. The overall objective of this thesis is achieved via solving two problems. The first problem is to develop a dynamic control model of quadrtor. In this thesis, a proportionalintegral-derivative controller (PID) based feedback control system is developed and implemented on MATLAB s Simulink. The PID controller helps track any given trajectory. The second problem is to design a trajectory that will fulfill the mission. The planed trajectory should make sure the quadrotor will scan the whole area without missing any part to make sure that the quadrotor will find the lost person in the area. The generated trajectory should also be optimal. This is achieved via making some assumptions on the form of the trajectory and solving the optimization problem to obtain optimal parameters of the trajectory. The proposed techniques are validated with the help of numerous simulations. iv

6 Acknowledgments At first I would like to thank my parents, who always supported and encouraged me. They always taught me I should not give up when I am in a difficult situation. Without their financial support, I could not finish my studies in US. I would like to thank my advisor Dr. Manish Kumar. He always gave me most useful suggestion when I had problem in my research. I am extremely fortunate to have him as an advisor. Also, I would thank my committee members, Dr. Mohammad Elahinia and Dr. Mehdi Pourazady. They gave me a lot of valuable comments and suggestions. Especially, I would like to thank my American parents Mr.and and Mrs. Elmer. They have been really helpful in improving my English and helping me adapt to the life in US. At last but not least, a thank you to Ruoyou Tan, Alireza and Sarim who are my labmates. Alireza gave me a lot of help at the beginning of my research for developing my Simulink code. Ruoyou helped analyse the research paper and guided me at every step. I am really thankful to have a very good group in our CDS lab. v

7 Table of Contents Acknowledgments... v Table of Contents... vi List of Figures... ix List of Symbols... xiii 1 Introduction Unmanned Aerial Vehicles (UAVs): An Introduction Problem Motivation Optimal Path Planning and Control Problem Document Outlines Literature Review UAV control system Coverage and Path Planning Problem Description Quadrotor dynamics model design Coverage and optimal path planning Quadrotor Dynamics and Control Model Dynamics Analysis Modeling Dynamic Model vi

8 4.1.3 Position Control Hover Controller Attitude Control Dynamics Model Simulation Trajectory Design and Optimization Trajectory design Optimization of Trajectory Parameters Segment 1: Curve Segment Segment 2: Straight line Segment 3: straight line Simulation Results Segment 1: Curve Segment Segment 2: Straight line Segment 3: Straight line Overall simulation Conclusions and Future work Conclusions Contributions Future Works References A Appendix A Parameters of the dynamics model B Appendix B The system for the dynamics simulation vii

9 Subsystem of the dynamics simulation viii

10 List of Figures 1-1 co-axial UAV in CDS Lab octocopter UAV model Parrot AR. Drone 1.0 Quadrotor CDS Lab UAVs Dubbin s Curve The nested control loops for position and attitude control Simplified illustration of rotor motion and control of a Quadrotor Coordinate systems and forces/moments acting on a quadrotor frame Diagram showing Pitch, Yaw, and Roll angle Simulation result showing the trajectory of the UAV Simulation result showing the x location of the quadrotor versus time Simulation result showing the y location of the quadrotor versus time Simulation result showing the z location of the quadrotor versus time...30 ix

11 4-8 Simulation result showing the desired (shown in red) and actual trajectory (shown in blue) of a quadrotor following the circular trajectory Simulation result showing the desired x (shown in red) and actual x (shown in blue) of the quadrotor following a circular trajectory Simulation result showing the desired y (shown in red) and actual y (shown in blue) of the quadrotor following a circular trajectory Simulation result of the quadrotor s motion following a set of desired locations The sketch map of Dubins Curve Sample coverage area The sketch map of path segment Path planning of the UAV for the coverage problem Classification of motions in the UAV trajectory Real path simulation result at the linear velocity equal to 0.1 m/s Real path simulation result at the linear velocity equal to 0.2 m/s Real path simulation result at the linear velocity equal to 0.3 m/s Real path simulation result at the linear velocity equal to 0.4 m/s Real path simulation result at the linear velocity equal to 0.5 m/s...43 x

12 5-11 Segment 2: Straight line The rotors speed simulation at the acceleration equal to 1.5 m/s The rotors speed simulation at the acceleration equal to 2.0 m/s The rotors speed simulation at the acceleration equal to 2.5 m/s Simulation result of the desired velocity and actual velocity in circle segment Simulation result of the desired path and actual path in circle segment Simulation result of the desired velocity and actual velocity in Segment 2: straight line Simulation result of the desired acceleration and actual acceleration in Segment 2: straight line Simulation result of the trajectory and actual path in Segment 2: straight line Simulation result of the desired velocity and actual velocity in Segment 3: straight line Simulation result of the desired acceleration and actual acceleration in Segment 3: straight line Simulation result of the trajectory and actual path in Segment 3: straight line The trajectory for coverage the whole area and the actual path Simulation result showing the area covered as the UAV tracks each of the segments xi

13 ...64 xii

14 List of Symbols W... World frame B... Body frame g...acceleration due to gravity Φ...Roll Angle θ... Pitch Angle ψ...yaw Angle r...position vector of the center of mass in the world frame m...mass of Quadrotor p, q, r...the components of angular velocity of the quadrotor k F...Propeller force constant k M... Propeller moment constant M i... The moment produced by each rotors F i...the force produced by each rotors K d, K p... Controller gains I... Inertia matrix of Quadrotor L... The length between the axis of rotors to the center of mass ω i... Speed of each rotor ω i... Differential motor speeds xiii

15 Chapter 1 Introduction 1.1 Unmanned Aerial Vehicles (UAVs): An Introduction An Unmanned Aerial Vehicle (UAV), commonly known as a drone is an aircraft without a human pilot on board. Its flight is controlled either autonomously by computers in the vehicle, or via remote control by a pilot on the ground or in another vehicle [1]. In 1917, Peter Cooper and Elmer A. Sperry invented the first UAV, named Sperry Aerial Torpedo. The size was equivalent to a normal size airplane. It took a payload of 300 pounds and flew 50 miles during its fight. There is a lot interest recently in smaller UAVs also called miniature aerial vehicles (MAVs). The development of technology, such as materials, electronics, sensors, and batteries has fueled the growth in the development of MAVs that are typically between 0.1 and 0.5 m in length and kg in mass [2]. Currently, well-known of the applications of the UAVs are all in the military. The example include MQ-1 Predator which is an unmanned aerial vehicle (UAV) built by General Atomics and used primarily by the United States Air Force (USAF) and Central Intelligence Agency (CIA). Similarly, the Northrop Grumman RQ-4 Global Hawk is primarily used for surveillance. The AeroVironment RQ-11 Raven is a hand-launched 1

16 remote-controlled small unmanned aerial vehicle (or SUAV) developed for the U.S. military [3]. There can be several designs for the UAVs. These include fixed-wing, quadrotors, co-axial, double rotors and octocopters. A fixed-wing UAV is designed based on the fixed-wing aircraft which is an aircraft capable of flight using wings that generate lift caused by the vehicle's forward airspeed and the shape of the wings. A quadrotor UAV consists of two pairs of counter-rotating rotors and propellers, located at the vertices of a square frame. It is capable of vertical take-off and landing (VTOL), but it does not require complex mechanical linkages, such as swash plates, that commonly appear in typical helicopters [4]. A co-axial UAV has a pair of rotors mounted on the same mast and with the same axis of rotation, but turning in different directions as shown in Fig Because of inclusion of a coaxial main rotor and exclusion of a tail rotor, which also means a smaller footprint, co-axial rotor designs improved the co-axial UAV to become more stable, more maneuverable, quieter and safer than traditional helicopters [5]. An octocopters UAV consist of eight rotors and propellers as shown in Fig An octocopter s counter rotating stabilization system is similar to but more powerful and efficiency compare with coaxial. 2

17 Figure 1.1 Co-axial UAV available in the Cooperative Distributed Systems (CDS) Lab at University of Toledo Copyright XiaoXiong (2014). Figure 1.2 Octocopter UAV * 3

18 In 2012, Andy Janetzko and his friend designed a new kind of UAV called hybrid quadcopter. It vertically takes off and lands (VTOL) as well as derives lift like fixed win airplanes. That way to the UAV achieves the highest possible aspect ratio and lift [6]. The UAV technology has opened sea of opportunities in its potential use in civilian domain. Civilian agencies or departments as well as civilian commercial companies can utilize this technology in emergency response, search and rescue, transportation, and surveillance, monitoring or inspection of infrastructure. For example on 15 th May, 2008, the earth quake in China thoroughly destroyed the communication network in the affected area. The Chinese government used UAVs set up as the temporary signal supplier. For the problem being solved in this thesis, a motivating example could be search and rescue of people or looking for a hot-spot in wildfire. In these cases, there is limited a priori information available other than the information that the lost person or hot-spot would likely be in a given area which needs to be spotted and located. For these situations, UAVs provide a safe alternative where a UAV with an onboard vision or thermal camera go through and scan this area. The images captured by the UAV can be sent back to a ground control station whether it can be processed either manually or autonomously to locate the object of interest. Most of the current UAV applications are conducted using fixed-wing UAVs and quadrotor helicopters. There is a lot of interest recently on quad-rotor UAVs. Quadrotor is a type of UAV which consists of four rotors in total, with two pairs of counter rotating, fixed-pitch blades located at the four corners of the aircraft [7]. Compared to a fixedwing UAV, a quadrotor has the flowing advantages: 4

19 1) Vertical takeoff and landing. A fixed-wing UAV needs time to glide or taxi which generates enough thrust to fly up. But in case of a quadrotor, there are four motors fixed on the frame. They can provide thrust for the quadrotor to fly up vertically and work with its gravity to make the quadrotor land vertically as well. 2) Ability to hover When the thrust and gravitational force the same and in opposite direction, the total force on the quadrotor is zero. So, it can hover in the air. Fixed-wing aircraft on the other hand, cannot hover. 3) Simple structure, and easily controllable The structure of quadrotor is very simple. It is combined with the frame and four motors. The rotations of rotors produce forces and moments whose magnitudes depend on the speed of roration. Hence, the only inputs to control a quadrotor are the speeds of four rotors. 4) Low level noise Four rotors provide the thrust for quadrotor. So the size of individual rotor as compared single rotor helicopters is not so big. This also reduces the noise. Fig. 1.3 shows a Parrot AR Drone available in Cooperative Distributed Systems (CDS) Lab at the University of Toledo. 5

20 Figure 1.3 Parrot AR. Drone 1.0 Quadrotor The following Fig. 1.4 shows the different types of UAVs available in the CDS lab. Figure 1.4 The UAVs available in the CDS Lab 6

21 1.2Problem Motivation The thesis' work has been motivated by the overwhelming UAV applications in civilian and military fields, which results from the advances in communication, computing, sensing, solid state devices, and battery technology. With the use of on-board sensors, this technology is able to provide a feasible solution for low-altitude and highresolution applications such as scientific data gathering, homeland security, search, and rescue, precision agriculture, forest fire monitoring, geological research and military reconnaissance. A large class of application involves the UAV to search for a ground event or target and communicate that information back to the ground control station. These tasks require the UAV to search the target/event in an optimal and autonomous fashion. The thesis focuses on this problem and presents a method to carry out an exhaustive coverage of a given area in an optimal fashion. 1.3 Optimal Path Planning and Control Problem This thesis can be divided in three parts: The first is, obtaining the dynamic and control model of the quadrotor which consists of the following: 1) Derivation of the dynamics. 2) Designing a suitable control system based on the dynamics. 3) Simulate the dynamics and control model using MATLAB Simulink. 7

22 The second part is, designing the trajectory or planning the path. Designing a trajectory is often based on the problem at hand. For this thesis, the task is to design a trajectory so that the quadrotor exhaustively covers the entire area. The thesis considers a rectangular area and develops a trajectory that covers the entire area. The trajectory consists of several segments as will be explained in the subsequent chapters. The third part is to optimize the parameters of the path. Typically, one would like to minimize the time required to complete the entire operation. As mentioned earlier, the trajectory consists of several segments. The objective of this part is to obtain optimal control actions such that the quadrotor follows the desired trajectory in order to complete the entire operation in minimum time given the various constraints imposed by the kinematics and dynamics of the quadrotor. 1.4 Document Outlines The outline of this document is provided below: Chapter 1: In this chapter, an introduction of this research is presented. The applications of UAV in the world and the concept of UAV are stated. The difference between fixedwing UAV and quadrotor is listed. Chapter 2: The background survey and related works are presented in this chapter. The recent research on UAV control system, optimal path planning and coverage problem are mentioned. 8

23 Chapter 3: This chapter presents details on control of UAV trajectory and optimal design problems. Chapter 4: This chapter presents the dynamics and control model design of the quadrotor. A quadrotor has a complex dynamics. This chapter provides an analysis of the dynamics of quadrotor in a step by step fashion and subsequently the model is implemented and tested using MATLAB Simulink. Chapter 5: This chapter provides a description of the design and selection of trajectory. For scanning the area of interest, a trajectory is designed. Furthermore, the chapter provides our approach to solve the optimal control problem of covering the entire area in the shortest time. Chapter 6: This chapter presents the conclusion and future works. The chapter lists the advantages and disadvantages of the method utilized in the thesis. Subsequently, scope of future work is described. 9

24 Chapter 2 Literature Review This chapter presents the background survey of the existing literature on optimal control and quadrotor control. UAVs belong to a class of non-linear systems which are inherently more difficult to control. Due to recent interest in UAVs and their potential applications, a lot of work has been carried out recently in the area of UAV control. In this thesis, a PID (proportionalintegral-derivative) control system was designed for controlling the UAV to follow a pre-defined trajectory. One aspect of the problem considered in this thesis is the coverage problem, which has been studied a lot recently. The interest is primarily governed by its several applications such as tracking unfriendly targets, surveillance and monitoring, and urban search and rescue in the aftermath of a natural or man-made disaster [8]. For this project, the aim is to scan the area and find the missing person or the person in danger. To solve the coverage problem, a path planning is needed. The last task is to optimize the path planning to minimize the time. In what follows, a brief description of current literature in all these different aspects of UAV control and navigation is provided. 2.1 UAV control system With the development of materials, electronics, sensors and batteries, micro Unmanned Aerial Vehicles size ranges from meters in length and kgs in 10

25 mass [9]. A class of UAVs with four rotors, Quadrotors, has become more and more popular now. A lot of work has been carried out to develop new methods to control them. The example includes HMX-4 which is a kind of quadrotor, which has a weight around 0.7 kg and is 76 cm in length between the rotor tips. The flight time is around 3 minutes [10]. GPS or other accelerometers cannot be added onboard because of the limitation of the weight for this kind of quadrotor [11]. The data of position and speed can be obtained through the cameras. For feedback, the quadrotor has early Micro Electro-Mechanical System (MEMS) gyro system for pilot-assist. The feedback linearization controller controls the altitude and yaw angle. Due to the drift of the qaudrotor in the x-y plane under the control of this controller, the backstepping controller is needed to help to control the position [12]. Another commonly researched testbed uses the proportionalintegral-derivative controller (PID controller) to control the attitude and position together [13]. AscTec Hummingbird is a typical model of this kind of quadrotor. The model has a wood or carbon fiber frame to make it strong and have lightweight around 0.5 kg. They have their own sensors to obtain the states. The controller adjusts the plant based on the difference between the desired and measured values [14]. The STARMAC testbed is another one used PID control system. The quadrotor has weight around kg and can carry on additional payload around 2.5 kg [15]. One capable used outdoor trajectory tracking model is named as X4-flyer [16]. The controller was developed for bounding the orientation of the vehicle and keeping it very small. The control strategy was based on Lyapunow function with the application of backstepping techniques [17].In this project, the dynamic model was obtained through Euler-Langrage method. The PID controller was used to control the quadrotor to hover at one position or track a trajectory. 11

26 2.2 Coverage and Path Planning Recently, the research on sensor coverage of a specified area has gathered much attention. The developments of novel kids of sensors drive the research in sensor-based coverage by robots. One of the challenges is to control the motion of robots or UAVs to drive the sensor to carry out specific task such as exhaustive coverage of an area. The geometric motion planning techniques allow the navigation problem come to researchers eyes [18]. Besides that, researchers pay more attention to the research on competitive algorithms which will be used for the autonomous systems [19]. These reasons promote the coverage problem to be popular. There has been a lot of work focused on developing planning strategies recently. The example includes Potvin s research on delivery systems [20], autonomous UAV path planning [21], and mobile sensor network [22]. There are several new applications of coverage problem that includes snow removal, mobile target tracking, surveillance, floor cleaning, and lawn mowing [23]. L.E. Dubins was the first person who did the research on the time-optimal paths of a simple airplane. The airplane model developed by him is named the Dubins curve [24]. Dubins curve always list as: LSL, RSR, RSL, LSR, RLR, and LRL. Here S means straight line segment, L is a circular arc to the left, and R stands for the circular arc on the right side. This is shown in Fig. 2.1 below: 12

27 Figure 2.1 Dubin s Curve He designed the shortest path by giving a characterization of time-optimal trajectories for a car with a bounded turning radius. With the application of Dubins curve, Reeds and Shepp finished another research to give a path with a minimum turning radius on a special kind of model that a car goes both forwards and backwards [25]. Boissonnat, C er ezo, and Leblond solved the same problem by using another kind of optimal control theory named Minimum principle of Pontryagin [26]. In this work, they obtained the shortest path between two known oriented points. After that, Summann and Tang developed their research with the application of modern geometric optimal control theory and classical control theory [27]. Balkcom and Mason did their research of the optimal trajectories for a kind of differential drive vehicles [28]. Based on the same to Reeds cost function, they solved other wheel-rotation trajectories. They derived 52 different minimum wheel-rotation trajectories and obtained the shortest path to solve the problem from 13

28 Chapter 3 Problem Description In this thesis, there are two main problems to be solved: trajectory tracking and trajectory generation. The first problem is to develop a dynamic control model of a quadrotor. This dynamic control model will allow the UAV to track any desired trajectory. The second problem is to obtain the desired trajectory. This involves first designing a path that will fulfill the mission given constraints of motion of the quadrotor. Once desired path has been obtained, then the task is to optimize the path with regard to kinematic parameters such as position, velocity, and acceleration in order to get the minimum time for the quadrotor to finish the coverage task. In what follows, first description of control model design is provided followed by the proposed approach of trajectory generation. 3.1 Quadrotor dynamics model design The overall control architecture of the quadrotor can be represented by the block diagram shown in Fig 3.1 [30] below: 14

29 Figure 3.1 The nested control loops for position and attitude control. As Fig. 3.1 shows the nested feedback control loops for the quad rotor. The inner control loop is used to calculate the pitch, roll and yaw in order to control the trajectory in three dimensions by using position control in the outer loop. The position control has to keep the position at the desired location by using the current and desired position as an input, while the attitude control is used to calculate the moments causing pitch, roll and yaw. The acceleration commands along x, y and z axis in position control are calculated from Proportional- Integral- Derivative feedback of the position error. The Proportional-Derivative controller is also used in inner loop to calculate the moments which are needed to be produced for four motors. 15

30 PID controller is a common controller which is now commonly applied in the industrial control system. It uses the error signal, its derivative and its integral to calculate the control action. 3.2 Coverage and optimal path planning The quadrotor is located at (x, y, z). To completely scan the desired area, first, we need to design the trajectory that can guide the quadrotor to go through the complete area once. Once the trajectory is obtained, it needs to be optimized so that the quadrotor takes minimum time to traverse it while obeying all the dynamical and kinematical constraints. The inputs of the quadrotor are the altitude, roll, pitch and yaw angle. It can be considered a system with the configuration or state variables denoted by: p ( x, y, z,,, ) (3.1) Where x, y, z are the 3-dimensional location and,, are the pitch, roll, and yaw angles. The dynamics of the quadrotor is: p g( p, u( t), t) (3.2) If f is the total drag force, I is the moment of inertia, the quadrotor dynamics is represented by the equation: 16

31 f sin m f sincos x m y f coscos g z m p b 2 2 ( 3 1 ) L I xx 2 2 b( 2 4) L I yy d( ) I zz (3.3) Where b, d are the thrust and drag moment constants respectively. L is the length of the frame of qudrotor. In the above equation, the control variable u(t) represent the speed of the rotors w i. The cost function is the total time required to finish the complete operation: T J( u) dt (3.4) 0 The optimization problem is: min Ju ( ) subject to p g( p, u( t), t) 0 and A(x, y, t)dt Ω T 0 covered. where A(x, y, t) is the footprint of the UAV and Ω is the desired area to be 17

32 Chapter 4 Quadrotor Dynamics and Control Model 4.1 Dynamics Analysis Modeling The rotors of the quadrotor are designed to provide the only force on the top of quadrotor. The rotation of rotors also introduce moments. Both the force and moment generated are dependent on the rotational speed of the rotor. The main challenge is to control the respective speeds of the four rotors in order to generate stable flight along a desired trajectory. 18

33 Figure 4.1 Simplified illustration of rotor motion and control of a Quadrotor Figure 4.1 is a simplified illustration of how the rotors control the motion of Quadrotor. The rotors1 and 3 spin in the anticlockwise direction and give a momentum in clockwise direction. The rotors 2 and 4 spin in the clockwise direction and give a momentum in anticlockwise direction. All rotors provide force in the upward direction perpendicular the plane of rotation. For the left figure in Fig. 4.1, the speeds of rotors 2 and4 are bigger than the speeds of rotors 1 and 3. So, the effective momentum will be in the anticlockwise direction. The Quadrotor will yaw anticlockwise. For the right figure in Fig. 4.1, four rotors have the same speed. The momentum in anticlockwise direction will balance the momentum in clockwise direction. The quadrotor will go upside when the sum of the lift forces is bigger than the gravity. 19

34 4.1.2 Dynamic Model Figure 4.2 Coordinate systems and forces/moments acting on a quadrotor frame In order to develop a dynamic model of the quadrotor, let us set the body frame to be on the quardrotor. As shown in Fig. 4.2, the origin of the body frame is set on the center at the point O of the quadrotor. One arm is selected as the X axis, and the other one is selected as the Y axis. 20

35 Figure 4.3 Diagram showing Pitch, Yaw, and Roll angle The world frame is marked as W and determined by X w, Y w, Z w as shown in the Fig The body frame is marked as B. The red chain lines in the Fig. 4.3 are combined as a simple schematic of quarotor. Each line stands for the rotors of quadrotor. The body frame is attached at the point O. Point O is the center of the mass of the quadrotor. The direction pointing from O to rotor No. 1 is defined as positive X B direction. While the direction forward to No. 3 is negative X B direction. Similar, the direction pointing from O to rotor No. 2 is defined as positive Y B direction and to rotor No. 4 is the negative Y B direction. Z B direction is the direction from the point O to the opposite direction of the gravity. In Fig. 4.3, rotate about z W by the yaw angle, ψ, rotate about the intermediate x 21

36 axis by the roll angle, Φ, and rotate about the y B axis by the pitch angle, θ. The rotation matrix for transforming the coordinates from B to W is given by cos cos sin sin sin cos sin cos sin cos sin sin R cos sin cos sin sin cos cos sin sin cos cos sin (4.1) cos sin sin cos cos If r denotes the position vector of the center of mass in the world frame, the acceleration of the center of mass is given by the equation: 0 0 mr 0 R 0 (4.2) mg Fi In the body frame, the components of angular velocity of the robot are p, q, and r. The relationship between these values and the derivatives of the roll, pitch, and yaw angles is: p cos 0 cos sin q 0 1 sin (4.3) r sin 0 cos cos Each rotor i produces a moment M i which is perpendicular to the plane of rotation of the blade. Rotors 1 and 3 produce the moments in the - z B direction. The moments produced by Rotors 2 and 4 are in the opposite direction: z B direction. The moment 22

37 produced on the quadrotor is opposite to the direction of the rotation of the blades so M 1 and M 3 act in the z B direction while M 2 and M 4 act in the opposite direction. The distance from the axis of rotation of the rotors to the center of the quadrotor is marked as L. Through weighing the individual components of the quadrotor, the moment of inertia matrix, I which is referenced to the center of mass along x B - y B - z B axes is obtained [18]. The angular acceleration is obtained by the Euler equations as shown below: p L F2 F4 p p I q L F3 F 1 q I q r M r r 1 M 2 M3 M 4 (4.4) Position Control In this thesis a proportional-integral-derivative controller (PID controller) is designed to control the quadrotor. The controller adjusts the process control inputs to minimize the error which is the value of the difference between a measured process variable and a desired set-point [19]. The roll and pitch angle were put as the inputs. With the use of a method similar to back stepping approach, two kinds of position control methods will be obtained. One is hover controller which will control the quadrotor to keep staying in a desired position. The other one is to control the quadrotor to track any trajectory in 3-D [20] Hover Controller The quadrotor will hover at one point when the nominal thrusts from the propellers are equal to the gravity, so: 23

38 mg Fi,0 (4.5) 4 And the motor speed will be: i,0 mg h (4.6) 4k F Let r T (t) be the trajectory position and ψ T (t) be the yaw angles that are being tracked. Note that ψ T (t) = 0 for the hover controller. The position error is given by: The commanded acceleration: i i, T i e r r (4.7) des r i can be calculated using the PID law as: des ri T ri kd i ri T ri k p i ri T ri ki i ri T ri,,,,,,, 0 (4.8) For hover, we have: r i, T ri, T 0 (4.9) The relationship between the desired accelerations and roll, pitch angles as shown: 24

39 r g cos sin des des des 1 T r g sin cos des des des 2 T r 8k F m des F h 3 T T (4.10) des 1 des des r1 sin T r2 cos T g des 1 des des r1 cos T r2 sin T g m F r 8k F h des 3 (4.11) Attitude Control For hover state, p, q, and r Φdot p, proportional-derivative control laws are used that take the form des d des des d des des d des k k p p p,, k k q q p,, k k r r p,, I p Lk qr I I xx F zz yy I q Lk pr I I yy F xx zz I r k zz M (4.12) (4.13) 25

40 From the equation above the result of the ω desire are obtainable. des h F des des des (4.14) 4.2 Dynamics Model Simulation In this section, numerical simulation results for validation of the dynamic and control model discussed in the previous section are presented. The parameters used for the simulation are shown below in the Table Kpx=1 Kpy=1 Kpz=1 Kdx=1 Kdy=1 Kdz=1 kf=6.11*10-8 N/(r/min 2 ). KM=1.5*10-9 N*m/(r/min 2 ) Km=20 m=1.08 kg g=9.8 m/s 2 L=0.22m Table Parameters of the dynamics model Based on the dynamic model of the quadrotor, the control model was developed in the MATLAB Simulink. The AppendixⅡhows details of the Simulink model of quadrotor. For the simulation, the quadrotor flies from initial location (0,0,0) to goal location (10,10,10) and hovers at the point (10,10,10).The distance units mentioned here 26

41 are in meters. The Fig. 4.4 shows the actual path taken by the quadrotor as it moves from the intial to the goal location z(m) y(m) x(m) Figure 4.4 Simulation result showing the trajectory of the UAV Fig. 4.5, Fig 4.6 and Fig 4.7 repectively show the plots of x, y, z locations of the quadrotor versus time as the quadrotor moves from the intial to the desired location. 27

42 x(m) t(s) Figure 4.5 Simulation result showing the x location of the quadrotor versus time 28

43 y(m) t(s) Figure 4.6 Simulation result showing the y location of the quadrotor versus time 29

44 y(m) t(s) Figure 4.7 Simulation result showing the z location of the quadrotor versus time In order to further verify the dynamics and control model with respect to tracking a desired trajectory (rather goal location as above), the quadrotor was commanded to move in a circle with center at (5, 0) and radius 5 m. The Fig. 4.8 shows the simulation result of the desired and actual trajectory of the quadrotor following a circular trajectory. The Fig. 4.9 shows the simulation result show the desired x and actual x for this circular tracking. The Fig 4.10 shows the simulation result show the desired y and actual y for this circular tracking. 30

45 6 4 2 y(m) x(m) Figure 4.8 Simulation result showing the desired (shown in red) and actual trajectory (shown in blue) of a quadrotor following the circular trajectory 31

46 Figure 4.9 Simulation result showing the desired x (shown in red) and actual x (shown in blue) of the quadrotor following a circular trajectory 32

47 Figure 4.10 Simulation result showing the desired y (shown in red) and actual y (shown in blue) of the quadrotor following a circular trajectory The control model was further verified showing quadrotor move from rest to a set of desired locations and land again. The Fig shows the simulation of quadrotor s motion. The quadtor took off at the point (0, 0, 0) flew up to the point (0, 0, 10) then turned and moved to the point (10, 10, 10) and then landed at the point (10, 10, 0), The unit used is meter. 33

48 z(m) y(m) x(m) Figure 4.11 Simulation result of the quadrotor s motion following a set of desired locations The above numerical simulations demonstrated that the quadrotor was able to navigate to any desired way-point locations and follow any desired trajectory. 34

49 Chapter 5 Trajectory Design and Optimization 5.1 Trajectory design As previously stated in the Chapter 4, the Dubins curve is composed of S straight line segment, circular arc to the left, L, and circular arc on the right side, R as shown in Fig. 5.1 shown: Figure 5.1 The sketch of Dubins Curve 35

50 It may be noted that the unit for distance considered in this thesis is meters. For this thesis, the desired area is identified as a square area L (length) * W (width) =50 m*50 m as shown in Fig. 5.2: Figure 5.2 Sample coverage area To plan path for such a coverage area, the each path segment is designed to be composed of a straight line segment, a semi-circular arc, followed by a straight line segment as shown in the Fig. 5.3: 36

51 Figure 5.3 The sketch of a segment of the path The overall path is obtained as shown in Fig Initial location of the UAV is (0, 5). The quadrotor is required to initiate from this point into the desired area. The quadrotor flies up in a straight line to reach the point (0, 55). Total distance for the linear segment is 50. Then the UAV turns right to follow a semi-circular arc with radius equal to 5. It may be noted that the sensor coverage/footprint area for the UAV is considered to be 10 meters in diameter. After the quadrotor reachs the point (10, 55), it then flies in a straight line. These motions are repeated to completely finish the coverage task. 37

52 Figure 5.4 Path planning of the UAV for the coverage problem 5.2 Optimization of Trajectory Parameters Once the path or trajectory has been planned, the next step is to obtain trajectory parameters such as velocity and acceleration so as to minimize the overall time required to finish the mission. It may be noted that the trajectory parameters need to be optimized while keeping in view the constraints of the quadrotor dynamics. The overall trajectory is comprised of three kinds of movements as shown in Fig.5.5: 38

53 Figure 5.5 Classification of motions in the UAV trajectory Segment 1: Curve Segment For the curve segment, a semi-circular trajectory is designed. As previously discussed in Chapter 4, a simulation was designed to track the circular trajectory. The circular trajectory would be followed at a constant speed. The challenge is to determine the maximum velocity that can be allowed in presence of the dynamic of quadrotor. This maximum velocity can be obtained through the experiment with the Simulation model. The designed curve is a semi-circular with radius equal to 5 m. The quadrotor will track this circle with a constant linear speed Vcircle. The maximum speed that can track 39

54 this circle can be found via performing simulations with increasing magnitudes of velocities. The results from these simulation experiments are shown in Fig.5.6 to Fig.5.10 which shows the trajectory followed by the UAV. The velocities were set to 0.1 m/s, 0.2 m/s, 0.3 m/s, 0.4 m/s and 0.5 m/s. Figure 5.6 Trajectory followed by the UAV for magnitude of velocity equal to 0.1 m/s 40

55 Figure 5.7 Trajectory followed by the UAV for magnitude of velocity equal to 0.2 m/s Figure 5.8 Trajectory followed by the UAV for magnitude of velocity equal to 0.3 m/s 41

56 Figure 5.9 Trajectory followed by the UAV for magnitude of velocity equal to 0.4 m/s Figure 5.10 Trajectory followed by the UAV for magnitude of velocity equal to 0.5 m/s 42

57 The maximum errors between these real path and trajectory are , , , and We can see, as expected, that the error increases the speed increases. Because the maximum acceptable error is 0.2, so 0.4 m/s is the maximum linear velocity chosen for the quadrotor to follow the semi-circular trajectory. The radius is 5 m. The constant magnitude of velocity to move on this trajectory is 0.4 m/s. For the half circle, the minimum time needed is: t circle Segment 2: Straight line * r 3.14* s (5.4) V 0.4 circle Figure 5.11 Segment 2: Straight line 43

58 These two straight line movements shown in the above Fig consists of two segments: acceleration motion and deceleration motion. For motion labeled 1 in the figure, the quadrotor flies into the desired area from the initial point (0, 5). First, the quadrotor flies with the acceleration a 1 for t 1 time. At time t 1 the velocity is equal to v 1. Then, the quadrotor switches to fly with the deceleration a 2 for t 2 time and reaches the point (0, 55). At time t 2, the velocity is v 2. This is the same velocity with which the quadrotor flies during the curved segment of motion. The quadrotor then accelerates and decelerates (as discussed in next Section: Segment 3). The final segment, shown as Number 2 in the above figure, is exactly inverse of the first segment. Finally, the quadrotor stops at the point (50, 5). Now, the question arises what should these acceleration and deceleration be so as to minimize the time take to travel this segment of path while ensuring that the UAV satisfies all its dynamics constraints. For this thesis, the method based on Langrage multiplier is used to obtain the maximum or minimum value of a mathematical function in presence of constraints. With the set of Langrage multiplier, a new equation set or objective function is obtained. For No. 1 straight line, the objective is to minimize the time, i.e.,: f ( t, t ) t t (5.5) The dynamics of the motion is obtained using kinematic equations for motion of particles, as follows: 44

59 v1 a1t s1 a1t 1 2 v2 a1t 1 a2t2 1 s a t t a t 2 s1s2 L (5.6) So: a1t 1 a1t 1t2 a2t2 L 2 2 v2 a1t 1 a2t 2 (5.7) And, also, the maximum acceleration constraint due to dynamics of the aircraft poses the following constraints: a a a 1 max a 2 max (5.8) To get the limited function: g1( t1, t2, a1, a2) a1t 1 a1t 1t2 a2t2 L 2 2 g2( t1, t2, a1, a2) a1t 1 a2t2 v 2 g3( t1, t2, a1, a2) a1 a max g 4( t1, t2, a1, a2) a2 amax 45 (5.9)

60 So, the augmented cost function is: F( t, t, a, a ) f ( t, t, a, a ) g ( t, t, a, a ) g ( t, t, a, a ) g ( t, t, a, a ) g ( t, t, a, a )(5.10) The optimization is carried by simultaneously solving: F / t1 0 F / t2 0 F / a1 0 F / a2 0 F / 1 0 F / 2 0 F / 3 0 F / 4 0 (5.11) In order to obtain the maximum value of the acceleration the quadrotor can provide, tests were done using the simulation model. The results are shown in the following figures. The values of the acceleration were set as 1.5 m/s 2, 2.0 m/s 2 and 2.5 m/s 2. The maximum rotor speed is set to 130 rad/s. As the Fig shown, the rotor speed will increase as the acceleration increase. The rotor speed required is bigger than 130 rad/s when the acceleration equal to 2.5 m/s 2. So the maximum acceleration the quadrotor can provide is 2.0 m/s 2. 46

61 Figure 5.12 The rotors speed simulation at the acceleration equal to 1.5 m/s 2 47

62 Figure 5.13 The rotors speed simulation at the acceleration equal to 2.0 m/s 2 Figure 5.14 The rotors speed simulation at the acceleration equal to 2.5 m/s 2 48

63 So, the maximum acceleration constraint is chosen as: Also set: amax 2 / 2 m s (5.12) L 50 m, v 0.4 m/ s (5.13) 2 The v2 is set to 0.4m/s from the circular segment of the motion. Substituting the above values, a set of function is obtained: 1 1 ( a1t 1 a1t 2) 2a ( a1t 1 a2t2) 2a (0.5 t1 t1t 2) 2t ( 0.5 t2 ) 2t a1t 1 a1t 1t2 0.5a2t2 L 0 a1t 1 a2t2 v2 0 a1 amax 0 a2 amax 0 (5.14) So: t t (5.15) The minimum time to cover the segment No. 1 straight line is t 1 +t 2 = =9.8 s. Because the motion of segment No.2 is the inverse motion of No.1, so the shortest time to cover the same is 9.8 s. 49

64 5.2.3 Segment 3: straight line The segment 3 is similar as to segment 2, the motion of quadrotor also comprises of an acceleration motion followed by a deceeration motion. However, in segment 3, the initial and final velocities both are 0.4 m/s. The objective function is: f ( t, t ) t t (5.16) as follows: The motion function is obtained using kinematic equations for motion of particles, v3 0.4 v4 v3 a3t3 1 s v t a t 2 v5 v4 a4t4 1 s v t a t 2 s3s4 L (5.17) So: a3t3 a4t t3 a3t3 (0.4 a3t3) t4 a4t4 L (5.18)

65 And also: a a a 1 max a 2 max (5.19) The limited function: g1( t3, t4, a3, a4) 0.4 t3 a3t3 (0.4 a3t3) t4 a4t4 L 2 2 g2( t3, t4, a3, a4) a3t3 a4t4 g3( t3, t4, a3, a4) a3 amax g4( t3, t4, a3, a4) a4 amax (5.20) So, the augmented cost function is:,,, (,,, ) (,,, ) (,,, ) (,,, ) (,,, ) 5.21 F t t a a f t t a a g t t a a g t t a a g t t a a g t t a a The optimization is carried by simultaneously solving: F / t3 0 F / t4 0 F / a3 0 F / a4 0 F / 1 0 F / 0 2 F 3 / 0 F / 4 0 (5.22) 51

66 From the simulation model: amax 2 / 2 m s (5.23) Also set: L 50 m, v 0.4 m / s, v 0.4 m / s (5.24) 3 5 Substituting the above values, a set of function is obtained: 1 1 (0.4 a3t3 a3t4 ) 2a (0.4 a3t3 a4t4) 2a ( t3 t3t4) 2t ( t4 ) 2t t3 a3t3 (0.4 a3t3) t4 a4t a3t3 a4t4 0 a3 20 a4 20 (5.25) So: t t s 4.8s (5.26) The minimum time to scan one of the straight lines in segment 3 is t 3 +t 4 = =9.6 s. 52

67 The desired area is set as L*W=50*50 square area. The trajectory for coverage scanning is as shown before in the Fig.5.4. The trajectory of this quadrotor is known. It made 5 half circle curves. It needed time t curve : tcurve 39.25* s (5.27) The quadrotor that flied into the area from (0, 5) to (0, 55) and moved out from (50, 55) to (50, 5), it needed time t segment2 : tsegment2 9.8*2 19.6s (5.28) Besides those above, the quadrotor also moves 4 times in a straight line same as the segment 3 as previously discussed. For this segment movement it need time t segment3 : tsegment2 9.6*4 38.4s (5.29) So the total time for the quadrotor covering L*W=50*50 square area is: t= s (5.30) 5.3 Simulation Results Segment 1: Curve Segment Fig shows the simulation result of the desired magnitude of velocity and actual magnitude of velocity in the circular segment of the trajectory. The purple line shows the desired magnitude of velocity which is 0.4 m/s. The yellow line shows the actual magnitude of velocity. The errors are less than 0.05 m/s. Fig shows the simulation result of the desired path and the actual path in circular segment of the 53

68 trajectory. Red line is the desired trajectory and the blue line shows the actual path. The actual time to track this segment is s. Figure 5.15 Simulation result of the desired velocity and the actual velocity in the circular segment 54

69 Figure 5.16 Simulation result of the desired path and the actual path in the circular segment Segment 2: Straight line Fig shows the simulation result of the desired magnitude of velocity (or speed) and the actual speed in tracking Segment 2 that represents a straight line. The red line shows the desired speed. The blue line shows the actual speed. The errors are less than 0.5 m/s. Fig.5.18 shows the simulation result of the desired acceleration and the actual acceleration in tracking Segment 2 of the trajectory. The red line shows the desired acceleration. The blue line shows the actual acceleration. Fig shows the simulation 55

70 result of the desired path and actual path in tracking Segment2: straight line. Red line is the desired trajectory and the blue line shows the actual path. The errors are less than 0.1m. The actual time to track this segment is 98 s. Figure 5.17 Simulation result of the desired velocity and the actual velocity in Segment 2 (straight line motion) 56

71 Figure 5.18 Simulation result of the desired acceleration and the actual acceleration in Segment 2 (straight line motion) 57

72 Figure 5.19 Simulation result of the desired trajectory and the actual path in Segment 2 ( straight line motion) Segment 3: Straight line Fig shows the simulation result of the speed and the actual speed in tracking the Segment 3 of the motion which is again a straight line. The red line shows the desired speed. The blue line shows the actual speed. The errors are less than 0.5 m/s. Fig.5.21 shows the simulation result of the desired acceleration and the actual acceleration. The red line shows the desired acceleration. The blue line shows the actual acceleration. Fig shows the simulation result of the desired path and actual path in tracking the 58

73 desired trajectory in Segment3 of the motion. Red line is the desired trajectory and the blue line show the actual path. The errors are less than 0.09 m. The actual time to track this segment is 96 s. Figure 5.20 Simulation result of the desired velocity and the actual velocity in Segment 3 ( straight line motion) 59

74 Figure 5.21 Simulation result of the desired acceleration and the actual acceleration in Segment 3 (straight line motion) 60

75 Figure 5.22 Simulation result of the desired trajectory and the actual path in Segment 3 (straight line motion) Overall simulation Fig shows the trajectory and the actual path for coverage the complete area. The blue line shows the desired trajectory and the red line show the real path. As shown in the figure, the real path closely tracked the trajectory. 61

76 Fig The desired and actual trajectory for coverage of the complete area The following Figs show the area covered as the UAV tracks each of the segments. The total time used for covering this area is s. 62

77 Figure 5.24 Simulation result showing the area covered as the UAV tracks each of the segments 63