Until now, we've discussed how to control a quadrotor given a specified trajectory. We now turn our attention to generating that specified trajectory. Given a three-dimensional environment, we want to be able to specify points in that three-dimensional environment and have that quadrotor plan obstacle free trajectories in that environment. Accordingly, we'll consider this very simple subproblem. And this problem is as follows, imagine you have a rigid body or a quadrotor that needs to go from a start position to a goal position. Of course, we're also interested in orientations. And in this problem, you may have intermediate positions that you want the rigid body or the quadrotor to go through. This is a very general problem, it arises in all contexts in robotics, and it's particularly important for motion planning for quadrotors. The general set up of this problem is as follows. We're given start and goal positions, optionally orientations. We might want the quadrotor to visit intermediate positions or waypoints which can also include orientations. In general, we'll insist that the trajectories that the quadrotors follow are smooth because the quadrotor is a dynamical system, it cannot follow arbitrary trajectories. This generally translates to minimizing a rate of change of input, as we will see soon. We'll be particularly interested in the order of the dynamical system. If you consider a robot with a kinematic model, in other words, you assume that you can arbitrarily specify velocities, that's a first order system. If you have a robot with second order dynamics, that means that you can arbitrarily specify accelerations. For third order systems, you should be able to control or specify or command the third order derivative which is called jerk. And likewise, a fourth order system involves specifying or commanding the fourth derivative, which is called snap. The order of the system determines the input. If it's an nth order system, in other words, you're specifying or commanding the nth derivative of position, you generally have boundary conditions on the first order, the second order, the third order, all the way up to the n-1th derivative. To solve this problem, we want to use tools from calculus of variations. Calculus of variations refers to a body of literature in mathematics that deals with a very simple problem. In this equation, you're trying to minimize an integral. You're trying to find the best function x of t, that minimizes a functional. The functional is the integral of L, which depends on the function x of t, its derivative, x dot of t, and of course, time. The goal in calculus of variations is to find the best function, x star of t. This problem arises in several settings. If you want to find the shortest distance path, you will use a functional which is the integral of x dot squared or the square of velocity. If you're trying to find the shortest time path, a problem that arises in optics, you will let L be the simple functional 1. Likewise, in mechanics, you usually refer to a functional that's obtained from the kinetic energy T and the potential energy V. In our case, we'll be interested in connecting two given points, x specified at time 0 and x specified at time T. We're looking for a trajectory, in fact, the best trajectory, and we restrict ourselves to differentiable curves, and there are, of course, many such differentiable curves. And again, we want to find something that minimizes a suitable functional. Independent of the functional, the optimal curve, must satisfy what are called Euler Lagrange equations. These are necessary conditions that must be satisfied by this optimal function. The Euler Lagrange equations involve partial derivatives and total derivatives. So you take the partial derivative of L with respect to x dot and the partial derivative of L with respect to x. You also need the total derivative with respect to time. Let's look at the Euler Lagrange equations for the simplest setting. Again, you're specified x at time 0, and you're specified x at time T. Let's take the functional to be the integral of x dot squared. In other words, the input is simply the velocity. We're going to try to minimize the square of this input, or the square of the velocity integrated through the entire trajectory. If we take the Euler Lagrange equations and let the functional L be equal to the square of x dot, you quickly find out that this equation reduces to a very simple differential equation, x double dot equal to 0, which you can then solve. The resulting equation is simply the equation of a straight line. Going back to the boundary conditions that are specified, if we substitute the fact that you want the straight line to start at x0 at time 0, and end at xT at time T, you can get the straight line equation. This is the case when you have a first order system with the input being the velocity, and it works for robots that have a kinematic model. Again, a kinematic model is one where you can specify the velocity, you can command the velocity. In general, the system might be characterized by higher order dynamics. So for an nth order system, you might not be able to specify the velocity or command the velocity. The system has inertia, and you can't willy-nilly specify velocities. For an nth order system, the input is the nth derivative of position. In this setting, the Euler Lagrange equation looks a little more complicated. But once again, it involves partial and total derivatives of L. If you take n=1, as we've seen, we get the shortest distance curve, the straight line. N=2 corresponds to minimizing a functional that involves the square of the acceleration. N=3 involves minimizing the square of the jerk. N=4 involves minimizing the snap. Accordingly, we get the minimum velocity trajectory, the minimum acceleration trajectory, the minimum jerk trajectory, and the minimum snap trajectory. One thing you might ask yourself is the discrepancy between n=1 and the other values of n. And n=1 really corresponds to the minimum velocity trajectory, and yet it yields us the shortest distance trajectory. Why is that?
