Description

Quadrotor Control in the Plane
1 Introduction
In this exercise, you will be implementing the PD controller discussed in lecture to control the motion of the quadrotor in the Y-Z plane. Before starting on this programming exercise, we strongly recommend watching the video lectures, completing the review questions for the associated topics, and reading through this handout.
Just as you’ve done in the previous programming exercise, you will need to download the starter code and unzip its contents into the directory in which you wish to complete the assignment.
2 System Model
2.1 Coordinate Systems
The coordinate systems and free body diagram for the planar model of a quadrotor are shown in Fig. 1. The inertial frame, A, is defined by the axes a2, and a3. The body frame, B, is attached to the center of mass of the quadrotor with b2 coinciding with the preferred forward direction and b3 perpendicular to the rotors pointing vertically up (see Fig. 1).
2.2 Dynamics
For a quadrotor modeled in the Y − Z plane, its orientation is defined by a roll angle, φ. It is assumed that its pitch and yaw angles are 0. You will need the rotation matrix for transforming components of vectors in B to components of vectors in A:
. (1)
We will denote the components of angular velocity of the robot in the body frame by φ˙:
AωB = φ.˙

Figure 1: Planar quadrotor model and the coordinate systems.
In the planer model of the quadrotor, we only consider the thrust force on two of the rotors. The quadrotor has two inputs: the thrust force (u1) and the moment (u2). u1 is the sum of the thrusts at each rotor
u1 = Σ2i=1Fi ,
while u2 is proportional to the difference between the thrusts of two rotors
u2 = L(F1 − F2) .
Here, L is the arm length of the quadrotor.
Let r = [y,z]T denote the position vector of planar quadrotor in A. The forces on the system are gravity, in the −a3 direction, and the thrust force, in b3 direction. Hence, by Newton’s Equations of Motion,
. (2)
The angular acceleration is determined by Euler’s equation of motion
Ixxφ¨ = L(F1 − F2) = u2
As a result, the system model can be written as,
(3)
3 Controller
3.1 Linearization
The dynamic model of the quadrotor (Eq. 3) is nonlinear. However, a PD controller is designed for a linear system. To use a linear controller for this nonlinear system, we first linearize the equation of motions about an equilibrium configuration.
In the case of the quadrotor, the equilibrium configuration is the hover configuration at any arbitrary position y0,z0, with zero roll angle. The corresponding thrust force needed to hover at this configuration is exactly mg, while the moment must be zero. Explicitly, the values of the related variables at the hover configuration are
y0,z0,φ0 = 0,u1,0 = mg,u2,0 = 0
To linearize the dynamics, we replace all non-linear function of the state and control variables with their first order Taylor approximations at the equilibrium location. In this case, the non-linear functions are sin(φ) and cos(φ). Near φ = 0, sin(φ) ≈ φ and cos(φ) ≈ 1
Let r denote a state variable, either y, z or φ. We can find the commanded acceleration of that state, ¨rc, corresponding to a (PD) controller as follows. Define the position and velocity errors as
ep =rT (t) − r ev =r˙T (t) − r˙
We want error to satisfy the following differential equation, which will result in convergence of the error for some value of kp and kd.
(r¨T (t) − r¨c) + kpep + kvev = 0
From this, we can see that (4)
r¨c = r¨T (t) + kpep + kvev,
where kp and kv are proportional and derivative gains respectively.
As a result, the inputs u1,u2, can be derived as: (5)
(6)
)) (7)
)) (8)
3.2 Hover Controller
Hovering is the special case of which the desired position is constant and the desired roll is zero. From rT (t) = r0 = [y0,z0]T ,φT (t) = φ0,r˙T (t) = ¨rT (t) = 0 we get:

3.3 Trajectory Controller
For trajectory following, given the desired trajectories for each state and their derivatives, rT (t),(r˙)T (t),(¨r)T (t), the inputs u1,u2 can be calculated using Equations 6-8.
4 Assignment
4.1 Files included in this exercise
[?] controller.m – Controller for planar quadrotor. evaluate.p – Code that evaluates your controller. runsim.m – Test script to be called for testing. simulation_2d.m – Simulation code that will be called by runsim. submit.m – Script which calls the evaluate function for getting submission results. trajecories/ – Folder includes example trajectories for testing your controller. utils/ – Folder containing helper functions which you can use.
? indicates files you will need to implement
4.2 Tasks
To test different trajectories, you will need to modify the variable trajhandle inside the runsim.m to point to the appropriate function. Examples are provided inside the runsim.m file.
4.3 Submission and Grading
To submit your results to our server, you need to run the command submit in your MATLAB command window. A script will then evaluate your controller on two trajectories and generate output files (files with the type .mat) to be uploaded to the web UI. There will be one output file for each test case. The evaluation is based on how well your controller can follow the desired trajectories. For each trajectory, we have upper and lower thresholds for the cumulative position error while following the trajectory. If your error is below the lower threshold, you get full points while if the error is larger than the upper threshold you get zero. If the error is in between the two, we just do a linear interpolation for the score. The thresholds for the two trajectories are shown in the following table:
Part Submitted File Error thresholds Points
Line trajectory line.mat 0.08, 0.15 20
Sine wave trajectory sine.mat 0.10, 0.20 30
Total Points 50