The project is divided into three parts:1. Mathematical modeling of the quadcopter platform: First the nonlinear equations of motion (EoM) are presented. You will be asked to linearize the...

1 ECE-3111: Systems Analysis Project I. PROJECT DESCRIPTION In this project, we will design a controller to control the quadcopter system using its model. The main idea is to control the position and attitude (aircraft orientation relative to the vehicle’s center of gravity) of a quadcopter by manipulating the angular velocities of its rotors. You will design a PID controller and a state space controller (Section 2 students only) that drives the quadcopter to a specified height. The quadcopter should able to fly within ±0.1 meters of the specified height. The project is divided into three parts: 1. Mathematical modeling of the quadcopter platform: First the nonlinear equations of motion (EoM) are presented. You will be asked to linearize the EoM to form a linearized system using which the transfer functions and state-space models can be derived. 2. Design a Proportional-Integral-Derivative (PID) and linear quadratic regulator (LQR) controllers to stabilize the quadcopter system. 3. Test the controller performance using Matlab. II. QUADCOPTER DYNAMICS Figure 1 shows the diagram of the quadcopter that you will have to control. Notice that the rotors are paired, and each pair rotates in a different direction. Motors 1 and 3 rotate clockwise, whereas motors 2 and 4 have a counter-clockwise rotation. When all the motors rotate at the same angular velocity, the torques τ1, τ2, τ3 and τ4 (these are the counter torques applied to the aircraft as a consequence of the motors rotation) will cancel each other out and the quadcopter will not spin about its zb-axis (ψ = 0). The quadcopter will hover when the angular velocities are such that the total thrust (f1 + f2 + f3 + f4) generated by the rotors is equal to the force of gravity. Figure 1. Quadcopter configuration. The roll, pitch and yaw angles are denoted by φ,θ and ψ respectively. Motor 1 and 3 rotate clockwise and motor 2 and 4 rotate counter-clockwise as indicated by the arrows with black dotted lines. f1, f2, f3and f4are the thrusts generated by the rotors about their centers of rotation. τ1, τ2, τ3 and τ4 are the torques applied to the aircraft (counter torques) are a consequence of the spinning of the rotors. In order to describe the movement of the quadrotor and its orientation, two frames of reference are used, the inertial frame and the body frame (see Figure 1). The inertial frame is defined as a stationary frame, with gravity pointing in the negative z direction. The body frame is defined by the orientation of the quadcopter, with the rotor axes pointing in the positive zb direction and the arms pointing in the xb and yb directions. The attitude of the quadcopter is determined by three angles, roll-φ, pitch-θ and yaw-ψ. These angles can be changed by adjusting the angular velocities of the rotors. Changes in the roll and pitch angles are accompanied by translational motion. A quadcopter is an underactuated system, since it only has 4 actuators (rotors) for controlling 6 degrees of freedom (three translational, x, y, z and three rotational, φ, θ, and ψ). Let’s denote the Melissa Cruz Melissa Cruz 2 angular velocities of the motors by ω1, ω2, ω3 and ω4, and the angular velocity with which the quadcopter hovers by ωhover. The vertical takeoff and landing motions (change in the z-axis) are obtained by equally augmenting or diminishing the angular speed of all motors with respect to hover. III. QUADCOPTER MODEL The translational motion of the quadcopter in the inertial frame is described by the following set of equations: m ẍÿ z̈  =  00 −mg +RT b + FD (1) where x, y and z are the coordinates of the position of the quadcopter in the inertial frame, m is the mass of the system, g is the acceleration due to gravity, FD is the drag force due to air friction, T b ∈ R3 is the thrust vector in the body frame, and R ∈ R3×3 is the rotation matrix that relates the body frame with the inertial frame and is defined as follows: R = cosψ cos θ cosψ sin θ sinφ− cosφ sinψ sinψ sinφ+ cosψ cosφ sin θcos θ sinψ cosψ cosφ+ sinψ sin θ sinφ cosφ sinψ sin θ − cosψ sinφ − sin θ cos θ sinφ cos θ cosφ  The drag FD due to air friction is modeled as a force proportional to the linear velocity in each direction FD = −kd ẋẏ ż  where kd is the air friction coefficient. The thrust fi generated by the ith rotor is given by the following expression fi = kω 2 i , for i = 1, ..., 4, where k is the propeller/rotor lift coefficient and ωi is the angular velocity of the ith motor. The total thrust T b generated by the 4 rotors (in the body frame) is given by T b = 4∑ i=1 fi = k  00∑4 i=1 ω 2 i  . Since the angular velocity of each rotor is typically proportional to the applied voltage, we have ω2i = cmv 2 i , for i = 1, ..., 4, where cm is a constant and v is the voltage applied to the rotor. The Euler’s equations for rigid body dynamics are defined as follows: Iω̇ + ω × (Iω) = τ (2) where “×” denotes cross product, I ∈ R3×3 is the inertia matrix, ω = [ωxωyωz]T is the angular velocity vector, and τ = [τφτθτψ] T is the vector of external torques, the diagonal inertia matrix is I = Ixx 0 00 Iyy 0 0 0 Izz  , where Ixx, Iyy and Izz are the moments of inertia of the quadcopter about the xb, yb and zb axes, respectively. After computing the cross product, equation (2) reduces toIxx 0 00 Iyy 0 0 0 Izz ω̇xω̇y ω̇z  = τφτθ τψ − (Iyy − Izz)ωyωz(Izz − Ixx)ωzωx (Ixx − Iyy)ωxωy  (3) The roll τφ and pitch τθ torques are derived from mechanics as follows τφ = L(f1 − f3) = Lk ( ω21 − ω23 ) = Lkcm ( v21 − v23 ) , τθ = L(f2 − f4) = Lk ( ω22 − ω24 ) = Lkcm ( v22 − v24 ) , where L is the distance between the rotor and the quadcopter center (radius). As it was discussed earlier, all the rotors apply torques to the aircraft about its zb-axis while they rotate. In order to have an angular acceleration about the zb-axis, the total 3 torque generated by the rotors has to overcome the drag forces. The total torque about the zb-axis, that is, the yaw torque is given by the following equation: τψ = b ( ω21 − ω22 + ω23 − ω24 ) = bcm ( v21 − v22 + v23 − v24 ) where b is the propellers drag coefficient. The nonlinear model of the quadcopter is given as follows: ẋ = vx (4) ẏ = vy (5) ż = vz (6) v̇x = − kd m vx + kcm m (sinψ sinφ+ cosψ cosφ sin θ) ( v21 + v 2 2 + v 2 3 + v 2 4 ) (7) v̇y = − kd m vy + kcm m (cosφ sinψ sin θ − cosψ sinφ) ( v21 + v 2 2 + v 2 3 + v 2 4 ) (8) v̇z = − kd m vz − g + kcm m (cos θ cosφ) ( v21 + v 2 2 + v 2 3 + v 2 4 ) (9) φ̇ = ωx + ωy (sinφ tan θ) + ωz (cosφ tan θ) (10) θ̇ = ωy (cosφ) + ωz (− sin θ) (11) ψ̇ = ωy (sinφ/ cos θ) + ωz (cosφ/ cos θ) (12) ω̇x = Lkcm Ixx ( v21 − v23 ) − ( Iyy − Izz Ixx ) ωyωz (13) ω̇y = Lkcm Iyy ( v22 − v24 ) − ( Izz − Ixx Iyy ) ωxωz (14) ω̇z = Lkcm Izz ( v21 − v22 + v23 − v24 ) − ( Ixx − Iyy Izz ) ωxωy (15) Notice that in this model the ground effects are not considered which means, in principle, there can be negative values for z-coordinate of the quadcopter. Let the state vector of the system be x ∈ R12 = [x y z vx vy vz φ θ ψ ωx ωy ωz]T , and the input and output vectors are u ∈ R4 = [v21 v22 v23 v24 ]T and y ∈ R6 = [x y z φ θ ψ]T , respectively, the model of the quacopter can be written in a compact way as ẋ = ξ (x,u) (16) y = Cx (17) where C ∈ R6×12 is the output matrix and is a nonlinear vector function defined by (4)-(15). Notice that the input vector is in terms of the squared voltages of the rotors, and therefore the controller system should compute v21 , v 2 2 , v 2 3 , and v 2 4 instead of v1, v2, v3, and v4. The maximum voltage that can be applied to the motors is 10 volt. To make sure that this constraint is not violated, the circuitry within the quadcopter incorporates a clipping mechanism. Observe that negative voltages would change the spinning direction of the motors and therefore would make the model invalid. Based on the previous considerations, it is clear that the input constraints of the system are given by 0 volt2 ≤ u1, u2, u3, u4 ≤ 100 volt2 where u1 = v21 , u2 = v 2 2 , u3 = v 2 3 and u4 = v 2 4 . In the present setup, the attitude (φ, θ, ψ) of the quadcopter is computed from the readings of a gyroscope, an accelerometer and a magnetometer installed in the aircraft. The position (x, y, z) of the vehicle is determined by a sensor. IV. ASSIGNMENT Solve the following questions and write a report that provides answers to the following questions. Problem #1 Linearization In order to design a controller using linear system analysis tools, it is necessary to have a linear approximation of the nonlinear model around an operating point. In this exercise an operating point, which is also an equilibrium point, is the one of a stationary flight when the quadcopter is in a hover position. - Determine the linearization/equilibrium point of the vehicle for φ = θ = ψ = 0 and x = y = 0, z = 8m. - Write down the linearized set of equations. Melissa Cruz Melissa Cruz 4 Figure 2. Parameters of the quadcopter. Problem #2 System Modeling - State Space & Transfer Functions 1) Compute the state space model with x = [x y z vx vy vz φ θ ψ ωx ωy ωz]T as the state, u = [u1, u2, u3, u4]T as the control input and y = [x y z φ θ ψ]T as the measurement. Write down your result in the following form ẋ = Ax+Bu y = Cx+Du Show A, B, C and D state space matrices. Write down the dimensions of A, B, C and D? 2) Compute the transfer functions from the inputs u1, u2, u3, and u4 to the outputs x, y, z, φ, ψ, θ. Use the formula to compute ijth transfer function Gij(s) = Ci(sI − A)−1Bj +D, where Ci is the ith row of C matrix corresponding to ith output and Bj is the jth column of B matrix corresponding to jth input u. Problem #3 Control Conversion Typical control structure of a Quadcopter looks like the one in Fig. 3. Given the measurements y = [x y z φ θ ψ]T (in certain cases you can measure angular rates, i.e., [φ̇, θ̇, ψ̇]T ) the control variables or inputs are φ, θ, ψ and z. This controller is called as attitude controller. The attitude controller computes the commanded roll, pitch, yaw commands based on the current roll, pitch, yaw and the desired (or reference) roll, pitch, yaw and altitude (height). The model that you designed in Problems 1 and 2 uses lower level control inputs of motor speeds. The roll, pitch, yaw and height of the quadcopter is decided by how much motor speeds are provided to four motors. So, there is a mapping between the motor speeds and the roll, pitch, yaw, altitude control commands. The mapping is as follows: uz uψ uθ uφ  =  0.5 0.5 0.5 0.5 1 0.5 1 0.5 0.5 1 0.5 0.5 1 0.5 0.5 0.5   ω21 ω22 ω23 ω24  (18) Use the transformation in (18) to convert the transfer function from z, φ, θ, ψ to u1,u2, u3 and u4 to z, φ, θ, ψ to uz,uψ , uθ and uψ . This is an algebraic conversion. Use the formula ω2i = cmv 2 i = cmui for i = 1, 2, 3, 4. Problem #4 PID Control Design and Analysis Design P, PD, and PID controllers for the transfer function from the altitude control command uz to the altitude z. Do it using the following steps: 1. Draw a block diagram of a closed loop system with a plant, controller and sensor transfer function blocks. Label all the signals. Include a disturbance input. 2. Draw a root locus using maltab ’rlocus’ command and determine if: Melissa Cruz Melissa Cruz Melissa Cruz 5 Figure 3. Quadcopter control structure a) P controller can stabilize the system? b) PI controller can