We're now ready to apply what we have learned to quadrotors. Let's recall again the dynamic equations of motion, the two sets of inputs, u1 and u2. And remember, we're now trying to generate trajectories in terms of the quadrotor position and trying to figure out how we want to control the motors to drive the vehicle to those trajectories. The quadrotor controller consist of two loops, first the outer position control loop that specifies u1 and then second, an inner attitude control loop that determines u2. In order to control the position, the inner loop, the attitude control loop has to work perfectly. Back to the equations, u1 and u2 are the two inputs. You can see from the first set of equations that the second derivative position depends on u1. u1 is a scalar quantity, there's only one input in u1, but you're trying to control the x, y and z components of acceleration. Also notice that the rotation matrix R, depends on roll, pitch, and yaw angles. The second set of equations relates the angular velocities, p, q, and r to the derivatives of the roll, pitch, and yaw angles. The third set of equations at the bottom relates the input u2 to the rate of change of angular velocity. From the two sets of equations at the bottom, it is clear that the second derivative of the rotation matrix depends on u2. Now, if I turn my attention back to the first set of equations, you can see that the derivative of velocity, or the acceleration, depends on u1, and this dependence is linear. But through R it also depends on u2. If I take into account the fact that this dependence of R on u2 is through the second derivative, as you see on the bottom, then I can combine these facts to show that the fourth derivative of position depends on u2. This might be easier to see if you look at the linearized model of the system. In the linearized model, the rotation matrix is the identity. The angular velocities are related to the rates of change of roll, pitch and yaw, through an identity matrix. And the non linear inertia dependent terms at the bottom disappear. You should be able to go through these equations and convince yourself that the second derivative of position is proportional to u1 and the fourth derivative of position is proportional to u2. In summary, the position control loop involves attitude control and because of that, the position control system leads to a fourth order system. To specify trajectories, we want to consider only those trajectories that are differentiable at least four times. This motivates the use of a minimum snap trajectory, which tries to minimize the fourth derivative of position integrated over the time history. We use minimum snap trajectories for motion planning for quadrotors. I want to show you several examples of minimum snap trajectories. I want to to remind you that all of this only works if you have a robust inner attitude control loop. Here you can see the vehicle being launched into the air from a variety of inertial conditions, and the vehicle settles to a hover position, independent of the initial configuration. Here, the vehicle starts off from a given configuration and then goes to an end configuration through an intermediate point. These circular trajectories are also minimum snap trajectories. In this example, the minimum snap trajectory has been designed to go through circular hoops, insuring that the result trajectory is dynamically feasible and safe. The same idea can be applied to other problems involving quadrotor flight. Here, inspired by birds that are able to gracefully coordinate their wings and their limbs, this quadrotor vehicle is able to coordinate its flight and a gripper at the end to grasp objects as it flies through the air at speeds of up to 2 to 3 meters per second. As another example, here, a quadrotor equipped with a special gripper at the end, is able to perch on near vertical surfaces to vertical surfaces. Again, planning minimum snap trajectories to reach the desired end configuration at a close to 0 velocity, but with the right attitude. Let's think a little bit about what happens when you have obstacles in the environment. Here is an example of a safe minimum snap trajectory. A rectangular robot is guided through a set of walls from an initial configuration to a final configuration. This trajectory is obtained by splicing together many minimum snap trajectories. This is very reminiscent of what we looked at earlier, where we synthesized cubic splines or nth order splines to go through many different wave points. In this case, there are no wave points, instead the robot knows where the obstacles are. How do you synthesize minimum snap trajectories in a known environment? Here's the simple approach to think about these problems. Imagine you have obstacles, and in this case I'm going to assume there's more than one quadrator flying through the obstacle filled environment at the same time. We're going to box off these quadrotors inside virtual rectangular parallel pipettes. We assume that these rectangular parallel pipettes are known, and we want to make sure that they don't intersect each other. In addition, there are many obstacles. We assume that every obstacle is convex and there are polyhedral models that can characterize the extent of each obstacle. If the obstacles are non-convex, we can always describe them as a union of convex obstacles. Let's consider a generic obstacle, obstacle o. Because each obstacle is a convex polyhedron, we want to look at collisions in terms of the rectangular parallel pipe that characterizes the quadrotor, and each of the faces characterizing the obstacle. For a generic face f on an obstacle o, for a generic time instant t sub k, we want to make sure that the convex polyhedron characterizing the robot, and the face f, do not intersect. That fact is written as a linear inequality. The inequality restricts the positions R that can be occupied by the robot, it of course assumes that we know exactly where the obstacle is and we know the normal N for each face F for all obstacles, in this case considering obstacle o. We can write similar equations for every phase, for every obstacle, and every robot. This gives us the set of equations at the top, except I've modified this equation by adding a positive term on the right hand side. This positive term consists of a large, positive constant M and a binary variable B. This binary variable is characterized by the obstacle o, the face f, and the time instant t sub k. The reason we want this extra term is quite simple. In order for the robot to avoid a particular obstacle o, it is enough that this inequality be satisfied for any one of the faces. It is not necessary that this inequality be satisfied for every face on the obstacle. That fact is captured in the inequality at the bottom of your screen. If there are n sub f faces, we want to make sure that at least one of these binary variables is 0. And that is captured in this inequality. This approach has now been used to synthesize trajectories for suspended payloads, where the vehicle is carrying a payload and the length of the suspended payload is larger than the height of the window it needs to go through. By carefully designing minimum snap trajectories, the robot is able to carry the payload through the narrow window without colliding with the environment. This same technique can be extended to multiple quadrotors. Imagine you have obstacles, you want quadrotor 1 to go to position 1, quadrotor 2 to go to position 2, quadrotor 3 to go to position 3, and you don't want these to intersect. Here's a simple example in which two quadrotors coordinate their flight in an obstacle filled environment, doing so safely. Once again, every trajectory that you see here is a minimum snap trajectory. This approach has now been extended to 16 to 20 quadrotors. Again, these trajectories are all a minimum snap trajectories, and they're guaranteed to be collision free. [SOUND]
