Description

3D Controller and Trajectory Generation
1 Introduction
In this exercise, you will be extending your PD controller from the previous assignment to control a quadrotor in 3D. You will then generate the time parameterized trajectories such that the quadrotor will navigate through all of the given waypoints. 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.
Again, you will need to download the starter code and unzip its contents into the directory in which you wish to complete the exercise.
2 System Model

Figure 1: Quadrotor model with the body-fixed and the inertial reference frames.
2.1 Coordinate Systems
The coordinate systems and free body diagram for the quadrotor are shown in Fig. 1. The inertial frame, A, is defined by the axes a1,a2, and a3 around the origin (O). The body frame, B, is attached to the center of mass of the quadrotor with b1 coinciding with the preferred forward direction and b3 perpendicular to the plane of the rotors pointing vertically up during perfect hover (see Fig. 1). These vectors are parallel to the principal axes. The center of mass is C. Rotor 1 is a distance L away along b1, 2 is L away along b2, while 3 and 4 are similarly L away along the negative b1 and b2 respectively.
Since bi are principal axes, the inertia matrix referenced to the center of mass along the bi reference triad, I, is a diagonal matrix. This matrix will be provided to you as variable params.I.
2.2 Motor Model
Each rotor has an angular speed ωi and produces a vertical force Fi according to
Fi = kF ωi2. (1)
Experimentation with a fixed rotor at steady-state shows that kF ≈ 6.11 × 10−8rpmN2. The rotors also produce a moment according to
Mi = kMωi2. (2)
The constant, kM, is determined to be about 1.5 × 10−9rpmNm2 by matching the performance of the simulation to the real system.
2.3 Rigid Body Dynamics
We will use Z − X − Y Euler angles to model the rotation of the quadrotor in the world frame. To get from A to B, we first rotate about a3 through the the yaw angle, ψ, to get the triad ei. A rotation about the e1 through the roll angle, φ gets us to the triad fi (not shown in the figure). A third pitch rotation about f2 through θ results in the body-fixed triad bi. You will need the rotation matrix for transforming components of vectors in B to components of vectors in A:
. (3)
We will denote the components of angular velocity of the robot in the body frame by p, q, and r: A ωB = pb1 + qb2 + rb3.
These values are related to the derivatives of the roll, pitch, and yaw angles according to
(4)
Newton’s Equations of Motion Let r denote the position vector of C in A. The forces on the system are gravity, in the −a3 direction, and the forces from each of the rotors, Fi, in the b3 direction. The equations governing the acceleration of the center of mass are
. (5)
We will define our first input u1 to be
u1 = Σ4i=1Fi.
Euler’s Equations of Motion In addition to forces, each rotor produces a moment perpendicular to the plane of rotation of the blade, Mi. Rotors 1 and 3 rotate in the −b3 direction while 2 and 4 rotate in the +b3 direction. Since the moment produced on the quadrotor is opposite to the direction of rotation of the blades, M1 and M3 act in the b3 direction while M2 and M4 act in the −b3 direction. L is the distance from the axis of rotation of the rotors to the center of mass of the quadrotor.
The angular acceleration determined by the Euler equations is
. (6)
We can rewrite this as:
. (7)
where is the relationship between lift and drag given by Equations (1-2). Accordingly, we will define our second set of inputs to be the vector of moments u2 given by:
u .
The angular acceleration is determined by Euler’s equation of motion as,
Ixxφ¨ = L(F1 − F2) = u2
3 Robot Controllers
3.1 The Nominal State
Our controllers are derived by linearizing the equations of motion and motor models (5 – 2) at an operating point that corresponds to the nominal hover state, r = r0, θ = φ = 0,

r,r˙
Figure 2: The position and attitude control loops.
ψ = ψ0, r˙ = 0, and φ˙ = θ˙ = ψ˙ = 0, where the roll and pitch angles are small (cφ ≈ 1, cθ ≈ 1, sφ ≈ φ, and sθ ≈ θ). At this state the lift from the propellers is given by:
,
The nominal values for the inputs at hover are u1,0 = mg, u2,0 = 0. Linearizing (5), we get:
(8)
Linearizing (6), we get:
(9)
If we assume the rotor craft is symmetric so Ixx = Iyy, we get:
.
In other words, the equations of motion are decoupled in terms of angular accelerations. Each component of angular acceleration depends only on the appropriate component of u2.
3.2 Position and Attitude Control
The control problem is to determine the four inputs, {u1,u2} required to hover or to follow a desired trajectory, zdes. As shown in Figure 2, we will use errors in the robot’s position to drive a position controller from (8) which directly determines u1. The model in (8) also allows us to derive a desired orientation. The attitude controller for this orientation is derived from the model in (9).
The transformation of the inputs into {u1,u2} is straightforward and is described in [1]. The inner attitude control loop uses onboard accelerometers and gyros to control the roll, pitch, and yaw and runs at approximately 1kHz, while the outer position control loop uses estimates of position and velocity of the center of mass to control the trajectory in three dimensions at 100-200 Hz.
Attitude Control We now present a proportional plus derivative (PD) attitude controller to track a trajectory in SO(3) specified in terms of a desired roll, pitch and yaw angle. Since our development of the controller will be based on linearized equations of motion, the attitude must be close to the nominal hover state where the roll and pitch angles are small.
Near the nominal hover state the proportional plus derivative control laws take the form:
u (10)
Position Control In the next subsection, we will present two position control methods that use the roll and pitch angles as inputs to drive the position of the quadrotor. In both methods, the position control algorithm will determine the desired roll and pitch angles, θdes and φdes, which can be used to compute the desired speeds from (10). The first, a hover controller, is used for station-keeping or maintaining the position at a desired position vector, r0. The second tracks a specified trajectory, rT (t), in three dimensions. In both cases, the desired yaw angle, is specified independently. It can either be a constant, ψ0 or a time varying quantity, ψT (t). We will assume that the desired trajectory:
zdes ,
will be provided by a trajectory planner as an input to specify the trajectory of the position vector and the yaw angle we are trying to track.
3.3 Hover Controller
For hovering, rT (t) = r0 and ψT (t) = ψ0. The command accelerations, ¨ri,des, are calculated from a proportional plus derivative (PD) controller. Define the position error in terms of components using the standard reference triad ai by:
ei = (ri,T − ri).
In order to guarantee that this error goes exponentially to zero, we require
(r¨i,T − r¨i,des) + kd,i(r˙i,T − r˙i) + kp,i(ri,T − ri) = 0, (11)
where ˙ri,T = r¨i,T = 0 for hover.
From (8) we can obtain the relationship between the desired accelerations and roll and pitch angles. Given that ∆θ = θ − θ0 = θ and ∆φ = φ − φ0 = φ, we can write

For hover, the last equation yields:
u1 = mg + mr¨3,des = mg − m(kd,3r˙3 + kp,3(r3 − r3,0)) (13)
The other two equations can be used to compute the desired roll and pitch angles for the attitude controller:
) (14a)
) (14b)
The desired roll and pitch velocities are taken to be zero.
pdes = 0 (15a)
qdes = 0 (15b)
Since the yaw, ψT (t) is prescribed independently by the trajectory generator, we get:
ψdes = ψT (t) (16a)
rdes = ψ˙T (t) (16b)
These equations provide the setpoints for the attitude controller in (10).
Note that the attitude controller must run an order of magnitude faster than the position control loop in order for this to work. In practice as discussed in [1], the position controller runs at 100Hz, while the inner attitude control loop runs at 1kHz.
3.4 3D Trajectory Control
Let rT denote the closest point on the desired trajectory to the the current position r, and let the desired velocity and acceleration obtained by differentiating the specified trajectory be given by r˙T and r¨T respectively. Let the unit tangent vector of the trajectory (unit vector along r˙T ) be tˆ. The unit normal to the trajectory, tˆ, is derived by differentiating the tangent vector with respect to time or arc length, and finally the unit binormal vector is the cross product bˆ = tˆ× nˆ. We define the position and velocity errors according to the following equations: ep = ((rT − r) · nˆ)nˆ + ((rT − r) · bˆ)bˆ
and ev = r˙T − r˙.
Note that here we ignore position error in the tangential direction and only considering position error in the plane that is normal to the curve at the closest point. Once again we calculate the commanded acceleration, ¨ri,des, from the PD feedback as shown in (11). In vector notation, this is:
(r¨T − r¨des) + kdev + kpep = 0, (17)
Finally we use (14 – 16) to compute the desired roll and pitch angles. Again we refer the readers to [1] for more details including experimental results obtained from these controllers.
4 Trajectory Generation
From lecture, we know that a necessary condition for a minimum snap trajectory is that it is a 7th order polynomial. If we are given a set of n+1 waypoints w0,…,wn, the minimum snap trajectory is a piecewise polynomial composed of n 7th order polynomials. Each polynomial piece pi travels between a pair of waypoints wi−1 and wi and takes a known amount of time Ti to complete, i = 1,…,n.
i
Let S0 = 0 and, for i = 1,…,n, Si = P Tk. That is, Si is the time it takes to reach
k=1
waypoint wi from waypoint w0. Then, the polynomial pi has the form:
(18)
To obtain the complete equation for the piecewise trajectory, we need to solve for all the coefficients αij. There are 8n such coefficients. These coefficients must satisfy a series of constraints. First, the polynomials must go through the given waypoints, giving 2n constraints:
pi(Si−1) = wi−1 for all i = 1,…,n pi(Si) = wi for all i = 1,…,n
Second, the quadrotor must start and stop at rest, giving two more constraints:
p˙1(S0) = p˙n(Sn) = 0
Next, the velocities and accelerations must be continuous between each segment, giving us an additional n − 1 constraints:
p˙i(Si) = p˙i+1(Si) for all i = 1,…,n − 1
We can also do this with the accelerations to get n − 1 more constraints:
p¨i(Si) = p¨i+1(Si) for all i = 1,…,n − 1
So far we have 4n constraints. We need 4n more in order to fully define all of the coefficients of our trajectory. How will we get these constraints? We get them from specifying continuity in higher derivatives of the trajectory at the intermediate waypoints (n − 1 constraints for each derivative), and from specifying the higher derivatives of the trajectory to be zero at the endpoints (2 constraints per derivative). So, to get 8n constraints, we can specify that the 3rd through 6th derivatives are continuous, giving us 4n − 4 more constraints:
) for all i = 1,…,n − 1, k = 3,…,6
and then we can specify that the acceleration and jerk are zero at the endpoints, bringing us up to 8n constraints:

To recap, the set of constraints needed to solve for the coefficients of the piecewise polynomial are:
pi(Si−1) = wi−1 and pi(Si) = wi for all i = 1,…,n (2n constraints) (19)
) = 0 for all k = 1,…,3 (6 constraints) (20)
) for all k = 1,…,6 (6n − 6 constraints) (21)
We can see that now, the number of constraint equations matches the number of unknown coefficients αij.
First, convert the equations to matrix form (you will have to work this out on your own):
Aα = b, (22)
where α is a vector containing the unknown coefficients αij and A and b are matrices representing all constraints. The coefficients can be obtained by solving:
α = A−1b (23)
Refer to the MATLAB tutorials from Week 1 if you need help solving this matrix equation.
5 Assignment
5.1 Files included in this exercise
[?] controller.m – Controller for a full 3D quadrotor.
[?] traj_generator.m – Trajectory generator for generating a trajectory passing through all the given waypoints. runsim.m – Test script to be called for testing.
simulation_3d.m – Simulation code that will be called by runsim. evaluate.p – Code that evaluates your controller.
submit.m – Script which calls the evaluate function for getting submission results. traj_helix.p – Pre-defined helical trajectory for testing your controller. traj_line.p – Pre-defined line trajectory for testing your controller. utils/ – Folder containing helper functions which you can use.
? indicates files you will need to implement
5.2 Tasks
5.2.1 Controller
You will need to complete the implementation of the PD controller in the file controller.m.
To test different trajectories, you need to modify variable trajhandle inside runsim.m to point to the appropriate function. Examples are provided inside the file runsim.m.
5.2.2 Trajectory Generation
5.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 first two parts evaluate your controller based on how well it can follow the given trajectories. Similar to the 2D quadrotor assigment, 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.2, 0.4 10
Helical trajectory helix.mat 2.0, 4.0 20
The third part requires you to provide your own trajectory generator and use your controller to follow a trajectory passing through a given set of waypoints. The evaluation for this part is based on how close the robot passes to the given waypoints and how long it takes to complete the trajectory. To count a waypoint as being cleared, the robot has to pass within 0.05m of the waypoint. You are given 5 waypoints that your trajectory has to pass through and clearing each one earns you 16% of the total score. The time taken for finishing the trajectory is responsible for the remaining 20%. Similar to the error thresholds for the other two parts, we have time thresholds here. A completion time of less than 15sec gets the full 20% while anything above 20sec results in no points for completion time.
References
[1] N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar,“The GRASP Multiple Micro-UAV Testbed,” IEEE Robotics and Automation Magazine, 2010.