In this section, we outline each step of the trajectory planning algorithm, based on Bernstein basis and B´ezier curve, in the flat output space. The related source code in MATLAB m files, as well as some C files for C/MEX, is available from https://github.com/dzhou-git/.
We first plan a trajectory in the flat output space, then by using the endogenous transformation as stated in Theorem 1, the full trajectory of state and input can be calculated. Theorem 1 indicates that the trajectory σ(t) in the flat output space needs to be sufficiently smooth up to its fourth time derivative, i.e., σ(t) ∈ C4. Specifically, the flat output trajectory σ(t) can be decoupled into position trajectory p(t) and yaw angle trajectory ψ(t), i.e., σ(t) = p(t)×ψ(t), and we require the position trajectory to be sufficiently smooth up to its fourth time derivative, and the yaw angle trajectory to be sufficiently smooth up to its second time derivative, i.e., p(t) ∈ C4, and ψ(t) ∈ C2, respectively. A position with a yaw angle in space, associated with with its derivatives (velocity, acceleration, jerk and snap), requiring the quadrotor to navigate through, is considered as a waypoint in this section.
Planning a straight line is a simplest way to plan a trajectory between a pair of waypoints. However, it dissatisfies with the smoothness requirement on the trajectory, the infinite derivative on the waypoints require the quadrotor to have full stop at the waypoints, causing instability in quadrotor control.
To satisfy with the smoothness requirement, we start with planning the position trajectory based on the B´ezier curve, in the form of equation (A.4), assuming the position, velocity, acceleration, jerk and snap information from a series of waypoints are known, as in Figure A·4.
The waypoints separate the trajectory to be planned into segments. Let’s consider two waypoints pi and pi+1, and assuming the quadrotor is required to fly through
Figure A·4: Waypoints and the trajectory passing through them.
pi at velocity vi, acceleration ai, jerk ji and snap si at time ti, and through pi+1 at velocity vi+1, acceleration ai+1, jerk ji+1 and snap si+1 at time ti+1, ti+1> ti.
Since the B´ezier curve in equation (A.4) is defined on t ∈ [0, 1], so the position trajectory p(t) between the two waypoints pi and pi+1 can be defined as
p(t) = ˆp(ˆt) = in our trajectory planning algorithm. Equation (A.5) needs to satisfy the following constraints on the two waypoints,
which is equivalent to implies that the B´ezier curve should be of degree larger than or equal to 9, in order to have the constraint (A.6), or constraints (A.7) to (A.16) not to be over-constrained.
With Bernstein polynomial basis of degree 9, the first five control points c0 to c4 can be uniquely determined from constraints (A.7) to (A.11), and the last five control points c5 to c9 can be uniquely determined from constraints (A.12) to (A.16).
In our code, we implemented the B´ezier curve of degree 13, such that the first five control points c0 to c4, and the last five control point c9 to c13, can be determined
from constraints (A.7) to (A.16), while the middle four control points, c5 to c8 can be determined by a means of optimization, e.g., minimum snap, as in (Mellinger and Kumar, 2011), in which the derivatives of waypoints are set free, and the control points are finally obtained by numerical quadratic programming (QP). In our case, however, with known the derivatives of the waypoints, the control points c5 to c8 can be obtained with a closed form solution by solving a similar minimum snap quadratic programming problem. The cost function is defined as
argmin
which is actually a quadratic function with respect to the elements of control points, i.e., cx,0, cy,0, cz,0, · · · , cx,13, cy,13, cz,13. Most importantly, cx,k, cy,k and cz,k are not correlated, i.e., there is no term such as cx,0· cy,1, and so on. With this observation, the control points are decoupled to three groups with regarding the cost function in equation (A.17), hence, for cx,0 to cx,13, we have a cost function
argmin
and because c0 to c4 and c9 to c13 can be uniquely determined from the constraint (A.7) to (A.16), the above cost function is quadratic in cx,5, cx,6, cx,7 and cx,8. The cost function (A.18) can easily be written as in the standard form of QP, as follows
argmin
c
1
2cTHc + hTc + const.,
in which c = [cx,5, cx,6, cx,7, cx,8]T, H is a 4×4 constant matrix, and h is a 4×1 constant vector. Notice that the QP with the cost function (A.18) has no more constraint for cx,5 to cx,8 hence the solution is
c = −H−1h.
The same procedure can be applied to cy,5to cy,8and cz,5 to cz,8, respectively, such that c5 to c8 are found.
With all the control points be found, the position trajectory from pi to pi+1 can be fully recovered from equation (A.4). Connections of the trajectory segments is then a full trajectory that passes through the pre-defined waypoints, smoothly.
The yaw angle trajectory ψ(t) is planned in the same way as the position trajectory p(t), however, it is only required to be sufficiently smooth up to its second time derivative, i.e., ψ(t) ∈ C2, hence the minimum degree of its corresponding B´ezier curve is 5. In our code, we set it with degree 9, hence it is capable to be optimized, by solving a so called minimum acceleration (mimic to minimum snap) QP problem.
An example of planned trajectory for a single quadrotor in its flat output space is shown in Figure A·5, in which we assign eight waypoints to ask the quadrotor to fly through at specified time instances. Notice that the local frames attached to the trajectory at the waypoints do not reflect the desired orientation of the quadrotor.
The orientation on the trajectory, i.e., the Euler angles trajectory, have to be calcu-lated from the endogenous transformation. Its result associated with the flat output trajectory in Figure A·5 is shown in Figure A·6.