Having looked at the two dimensional quadrotor, we now turn our attention to the three dimensional model, the dynamics, and we'll talk about trajectory planning and control. You'll recall that we attach a body fixed frame to the quadrotor and then we have an inertial frame. The state of the quad rotor consists of position and orientation. The position being the position vector of the center of mass. And the roll pitch and yaw angles telling you what the orientation is The state of course, is composed of the configuration a six-dimensional vector and its rate of change, in other words, the velocity. The roll, pitch and yaw angles follow our usual convention. First the yaw above the z axis And then the roll and pitch as we've seen before. Recall that the angular rates in the body frame, p q and r are related to the derivatives to the roll, pitch and yaw angles through this coefficient matrix. These set of equations tell you everything you need to know about the kinematics of the vehicle. What we´d like to do is to track arbitrarily specified trajectories in three dimensions. We assume we´re given a trajectory, but this trajectory now consists of a position vector that varies as a function of time, and the yaw angle, which varies as a function of time as well. We want this four dimensional vector to be differentiable, and we want to be able to obtain not only its derivative, but also its second derivative. As before we'll be interested in the difference between the desired position and the actual position. But now we're also interested in the difference between the desired yaw angle and the actual yaw angle. That gives us the error vector and its derivative. And again, we want the error vector to go exponentially to zero. This will allow us to calculate the commanded acceleration, whether it'd be the second derivative of the position vector, or the second derivative of your angle. Let's go back and look at the equations of motion, and the nested feedback loop we discussed earlier for the two dimensional quad rotor model. As in the planar case we have nested feedback loops. The inner loop corresponding to attitude control, and the outer loop corresponding to position control. In the inner loop, we'll specify the orientation either through a rotation matrix or a series of roll, pitch, and yaw angles. Will feedback the actual attitude and the angular velocity, or the roll, pitch and yaw angles and the angular rates. And from that, we want to calculate the input u sub 2. u sub 2 is a function of the thrust and the moments that we get from the motor speeds. And the equations of motion at the bottom, we have to calculate what u sub two is, based on the desired attitude. We now turn our attention to the outer loop, which is a position feedback loop. In this position feedback loop, we look at the specified position vector and the specified yaw angle. We look at the actual position and the actual velocity. And from that we calculate u1, u1 is essentially the sum of all the thrust forces. Rather than looking at the general trajectory following problem, let's focus on attention to a very specific case, the case of hovering. So in hovering, the position and orientation are fixed. In other words, all velocities are zero. Further, the roll and pitch angles are equal to zero. We want to look at small perturbations around the hover position. Accordingly, we'll linearize the dynamics around this current configuration. In order for the hover position to be one of equilibrium, the input u2 has to be 0, the rates p, q, and r are equal to 0, and their derivatives are equal to 0. Likewise, the sum of the thrust has to compensate the weight, but the yaw angle can be non-zero as long as it's fixed. Linearizing around this point, means that we assume that u1 is almost equal to mg. We assume the roll and pitch angles are close to zero, and we assume that the yaw angle is fixed, or close to fixed at a given value psi size zero. If you linearize the equations you'll end up with the simplified equations at the bottom for x double dot and y double dot. This allows us to design the inner feedback loop, by simply specifying u sub two using a proportional plus derivative controller. Here we assume that we have the commanded roll, pitch, and yaw angles and their derivatives. And all we require for feedback are the actual roll, pitch, and yaw angles, and their derivatives. Let's turn our attention to the outer feedback loop. If you look at the position equations of motion and you linearize them, you'll find that the expressions for the first two components of acceleration can be written in the form shown below. Using these linearized equations of motion we now turn our attention to the error equation that describes the error in position. Recall that we want this error to satisfy a second order differential equation. In this equation is a set of terms that have to do with the desired trajectory, and a set of terms with the actual trajectory that's being followed. From this equation we want to calculate the commanded acceleration. The commanded acceleration will then tell us what thrust we want to apply with the rotors. The only thing that's left now is to determine the commanded roll, pitch, and yaw. We do that using the linearized equations. If I know what the commanded acceleration needs to be in the X and Y direction, I can calculate the roll and pitch angles and their derivatives. And similarly I can determine the commanded yaw angles.
