# Sequential Convex Programming for Nonlinear Optimal Control in UAV Trajectory Planning
## 2 Dynamic Model
### Objective Function  
The control inputs for a quadcopter consist of the total thrust produced and the angular velocities. This study examined a normalized thrust force expressed in units of acceleration for the sake of simplicity. \[\mathbf{a}=\mathbf{R} \begin{bmatrix}0\\ 0\\ f\end{bmatrix} + \begin{bmatrix}0\\ 0\\ -g\end{bmatrix}  \tag{12}\]  
where \(\mathbf{R}\) represents the rotation matrix that accurately describes the rotation between the inertial and body-fixed frames. The quadcopter is described by six degrees of freedom. The translational position \((x_{1},x_{2},x_{3})\) is measured in the inertial coordinate system, as shown in Figure 1. The quadcopter attitude \(\mathbf{b}\) is defined by the rotation matrix \(\mathbf{R}\). The rotation matrix is defined such that when multiplying a vector \(v\) in the quadcopter coordinate \(\mathbf{b}\) system with it, the same vector, described in the inertial coordinate system \(\mathbf{i}\), is obtained. \[{}^{i}v={}_{b}^{i}\mathbf{R}^{i}v \tag{13}\]  
The input thrust \(\mathbf{f}\) can be calculated by applying the Euclidean norm to Equation (12), as follows:  
\[f=\mathbf{a}-\mathbf{g} \tag{14}\]  
where \(\mathbf{g}\) is the acceleration due to gravity and is equal to \((0,0,-\mathbf{g})\) in its vector form. To determine the jerk, it is necessary to express \(\dot{\mathbf{R}}\) in terms of the known parameters. The expression of \(\dot{\mathbf{R}}\) can be obtained by using the rotation matrix, \(\mathbf{R}\), and the skew-symmetric matrix of body rates, as follows:  
\[\dot{\mathbf{R}}=\mathbf{R} \begin{bmatrix}0&-\omega_{3}&\omega_{2}\\ \omega_{3}&0&-\omega_{1}\\ -\omega_{2}&\omega_{1}&0\end{bmatrix}  \tag{15}\]By applying Equations (13) and (15) and taking the derivative of the acceleration, the jerk can be calculated as follows:  
\[\mathbf{j}=\dot{\mathbf{R}} \begin{bmatrix}0\\ 0\\ f\end{bmatrix} +\mathbf{R} \begin{bmatrix}0\\ 0\\ \dot{f}\end{bmatrix}  \tag{16}\]  
\[\mathbf{j}=\mathbf{R} \begin{pmatrix}\begin{bmatrix}0&-\omega_{3}&\omega_{2}\\ \omega_{3}&0&-\omega_{1}\\ -\omega_{2}&\omega_{1}&0\end{bmatrix}\begin{bmatrix}0\\ 0\\ f\end{bmatrix}+\begin{bmatrix}0\\ 0\\ \dot{f}\end{bmatrix}\end{pmatrix}  \tag{17}\]  
Therefore, the cumulative thrust exerted by all agents at every time step can be selected as the cost function, as follows:  
\[f_{0}= \begin{bmatrix}\sum\limits_{l=1}^{T}\sum\limits_{i=1}^{N}\mathbf{a}_{i}^ {t}+\mathbf{g}_{2}^{2}\\ \end{bmatrix}  \tag{18}\]  
Equation (16) can be formulated as a quadratic equation, with \(\mathbf{G}\) representing the gravity vector of each agent at every time step, i.e., \(\mathbf{G}\in\mathbb{R}^{3 NT}\), as follows:  
\[f_{0}=\mathbf{X}^{T}\mathbf{X}+2\mathbf{G}^{T}\mathbf{X}+\mathbf{G}^{T} \mathbf{G} \tag{19}\]  
Equation (18) is expressed in a quadratic form. Hence, employing this cost function alongside appropriate affine equality/inequality constraints is feasible. Therefore, this issue can quickly be resolved using existing quadratic programming solvers. Once the acceleration of the agents is determined at each time step, the total thrust force may be calculated using Equation (14). By rearranging Equation (16), we may derive the following equation:  
\[ \begin{bmatrix}\omega_{2}\\ -\omega_{1}\\ 0\end{bmatrix} =\frac{1}{f} \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&0\end{bmatrix} \mathbf{R}^{T}\mathbf{j} \tag{20}\]  
The absence of the third component of body rates, \(\omega_{3}\), can be explained by observing that a rotation about the \(\omega_{3}\) axis does not impact the translational acceleration. The system inputs for a trajectory defined in its jerk are given using Equations (14) and (20), with one degree of freedom left in \(\omega_{3}\). This could be fully specified if the total attitude of the quadcopter, including rotation around the thrust axis, is also known. For simplicity, we will assume that the value of \(\omega_{3}\) is zero and that this rotation is insignificant.