[MUSIC] Now that we understand how to write the equations of motion for a quadrotor, let's turn our attention to the model for a planar quadrotor. The equations of motion that you saw earlier have now been rewritten in the form that you see here. At the bottom, you see the state's space description of the quadrotor, the 6 by 1 vector. In the bottom right, you see the derivative of a state vector. The first three elements of this derivative consist of velocities. The bottom three elements consist of accelerations. This is just a compact way of writing the equations of motion. On the right hand side, you'll see two inputs, u1 and u2. This vector is the input that can drive our dynamical system. By specifying appropriate u1 and u2, we can change the state of the quadrotor and get it to go where we want it to go. If you look at the equations of motion, you will notice that they are non-linear. The non-linearity here comes only from the fact that we have the sine of phi and cosine of phi. Let's consider these equations of motion at a hover configuration. We define a hover configuration at y equal to 0, z equal to 0, and phi equal to 0. The hover configuration can be at equilibrium only if the thrust vector, u1, opposes the gravity vector. So, at hover, the nominal value for the input you want must equal mg. Further the nominal value of u2 must be equal to 0. If it were not, then the vehicle would try to accelerate. Next we'll pursue a linearization of this nonlinear model. We'll call this the linearized dynamic model. We do this by recognizing that sine of phi and cosine of phi are the two principle sources of nonlinearity. And we ask how these nonlinear functions behave in close proximity to the hover configuration. So when phi is close to 0, sine phi behaves roughly like phi. And cosine phi is roughly constant. If you replace sine phi by phi and cosine phi by 1, you get the linearized equations that you see below. Let's now look at the control problem. Imagine a plane or quadrotor trying to follow a specified trajectory, r sub T. r sub T denotes a position vector. The fact that it's changing in time is denoted by the function r sub T as a function of t on the top right. Of course, this means that y and z are both changing as functions of time. Let's also assume that this function is differentiable. We can calculate the velocity and differentiate it again to calculate the acceleration. We're interested in taking this vector, r desired minus r, and reducing it to 0. r desired being the point on the trajectory that we wanna follow, and r being the current point. Let's define the error vector as r desired minus r, r desired being the same as r sub T. The velocity error is given by the derivative of the position error. And what we really want is for this error to decay exponentially to 0. As we've seen before, if you take the error term and make it obey a second order linear differential equation with positive coefficients, you guarantee that the error goes exponentially to 0. So we command an acceleration which forces this error term to obey the second order differential equation that you see at the bottom. This idea can be extended to develop a control structure that's hierarchical. Before you look at the block diagram in its entirety, I want you to focus on this inner loop, which is the attitude controller. Imagine you're just trying to control the attitude of the robot. In other words, you're trying to control this roll angle, phi, you have it follow a specified trajectory. What that means is, you wanna synthesize u2, such that phi goes to some desired value. That desired value is shown as the reference input, phi sub c. Of course, as before, we assume that phi sub c is differentiable. So you can calculate the derivative of phi sub c. And likewise, you can calculate the derivative of phi. On the bottom left, you see the equations of motion. If you look at the last equation you can essentially get the relationship between u sub 2 and phi double dot. This is a linear second order differential equation in one variable, and it's not too hard to determine u sub 2 so that phi converges to phi sub c. Let's assume we can do that and go to the next subproblem. Focus on the equation on the top left. We wanna follow a specified trajectory. Let's focus on y desired, the y component of that trajectory. If I set the roll angle phi to be given by y double dot divided by g with a negative sign, that essentially ensures that my roll angle will track the desired acceleration in the y direction. So, in summary, the inner loop is designed so that phi converges to phi sub c, and phi sub c is obtained from the y component of the desired trajectory. Finally, the z component tells me how to calculate u1. I take the middle equation, I take the desired trajectory, looking specifically at the z component. And I can write u on such that the z component tracks the desired z value. So I've now obtained two equations, one for u1 and one for u2. If I slap everything together, I have actually three equations. One that tells me what phi sub z is at the bottom. From that, I get u sub 2, and then I get u sub 1 from that. So these are the three control equations I can use to drive a quadrotor, assuming my assumption that it's close to the operating point is valid and assuming that it stays on the y-z plane. The only thing left to do is to calculate the constants, these are the gains, and there's actually six of these that need to be calculated.
