## 3 Motion Planning
### Trajectory Generation  
For optimal trajectory generation, the following functional can be used:  
\[\chi(t)=argmin_{(x(t))}\int\limits_{0}^{T}\mathcal{L}\Big{(}x^{n},x^{n-1},\ x^{n-2},\ldots\ldots,\dot{x},x,t\Big{)}.dt \tag{43}\]  
In order to generate optimal trajectory \(\chi(t)\), the following Euler-Lagrange equation must be satisfied:  
\[\frac{\partial\mathcal{L}}{\partial x}-\frac{d}{dt}\bigg{(}\frac{\partial \mathcal{L}}{\partial\dot{x}}\bigg{)}+\frac{d^{2}}{dt^{2}}\bigg{(}\frac{ \partial\mathcal{L}}{\partial\dot{x}}\bigg{)}+\ldots+(-1)^{n}\frac{d^{n}}{dt^ {n}}\bigg{(}\frac{\partial\mathcal{L}}{\partial x^{(n)}}\bigg{)}=0 \tag{44}\]  
For a quadrotor UAV, \(n\) = 4 inputs create a fourth-order system with \(x\) and \(y\) states, and this can be proved by the following differential flatness theorem. **Theorem 1**. : _The inputs of the quadrotor are the function of selected flat outputs and their derivatives._  
**Proof of Theorem 1. ** Recalling translational states from Equation (9) and assuming the linearized equation as:  
\[m\ddot{x}=(\theta\cos\psi+\phi\sin\psi)U_{1} \tag{45}\]\[m\ddot{y}=(\theta\sin\psi-\phi\cos\psi)U_{1} \tag{46}\] \[\ddot{z}=-g+(\cos\phi\cos\theta)\frac{1}{m}U_{1} \tag{47}\]  
The fourth derivative is taken as:  
\[mx^{(iv)}= (\theta cos\psi+\phi\sin\psi)\ddot{U}_{1}+2(\dot{\theta}\cos\psi- \theta\sin\psi\dot{\psi}+\dot{\phi}\sin\psi+\phi\cos\dot{\psi}\dot{\psi})\dot{U} _{1} \tag{48}\] \[+(\ddot{\theta}\cos\psi-\dot{\theta}\sin\psi\dot{\psi}-\theta\sin \psi\dot{\psi}-\theta\cos\psi\dot{\psi}^{2}+\ddot{\phi}\sin\psi\] \[+\dot{\phi}\cos\psi\dot{\psi}+\theta\cos\psi\ddot{\psi}-\phi\sin \psi\dot{\psi}^{2})U_{1}\] \[my^{(iv)}= (\theta sin\psi-\phi\cos\psi)\ddot{U}_{1}+2(\dot{\theta}\sin\psi+ \theta\cos\psi\dot{\psi}\dot{\psi}-\dot{\phi}\cos\psi+\phi\sin\psi\dot{\psi}) \dot{U}_{1}\] (49) \[+(\ddot{\theta}\sin\psi+\dot{\theta}\cos\psi\dot{\psi}+\theta \cos\psi\dot{\psi}-\theta\sin\psi\dot{\psi}^{2}-\ddot{\phi}\cos\psi\] \[\frac{\partial\mathcal{L}}{\partial x}-\frac{d}{dt}\bigg{(}\frac{\partial \mathcal{L}}{\partial\dot{x}}\bigg{)}+\frac{d^{2}}{dt^{2}}\bigg{(}\frac{ \partial\mathcal{L}}{\partial\dot{x}}\bigg{)}+\ldots+(-1)^{n}\frac{d^{n}}{dt^ {n}}\bigg{(}\frac{\partial\mathcal{L}}{\partial x^{(n)}}\bigg{)}=0 \tag{44}\]0 \[\frac{\partial\mathcal{L}}{\partial x}-\frac{d}{dt}\bigg{(}\frac{\partial \mathcal{L}}{\partial\dot{x}}\bigg{)}+\frac{d^{2}}{dt^{2}}\bigg{(}\frac{ \partial\mathcal{L}}{\partial\dot{x}}\bigg{)}+\ldots+(-1)^{n}\frac{d^{n}}{dt^ {n}}\bigg{(}\frac{\partial\mathcal{L}}{\partial x^{(n)}}\bigg{)}=0 \tag{44}\]1 (50) \[\frac{\partial\mathcal{L}}{\partial x}-\frac{d}{dt}\bigg{(}\frac{\partial \mathcal{L}}{\partial\dot{x}}\bigg{)}+\frac{d^{2}}{dt^{2}}\bigg{(}\frac{ \partial\mathcal{L}}{\partial\dot{x}}\bigg{)}+\ldots+(-1)^{n}\frac{d^{n}}{dt^ {n}}\bigg{(}\frac{\partial\mathcal{L}}{\partial x^{(n)}}\bigg{)}=0 \tag{44}\]2 \[\frac{\partial\mathcal{L}}{\partial x}-\frac{d}{dt}\bigg{(}\frac{\partial \mathcal{L}}{\partial\dot{x}}\bigg{)}+\frac{d^{2}}{dt^{2}}\bigg{(}\frac{ \partial\mathcal{L}}{\partial\dot{x}}\bigg{)}+\ldots+(-1)^{n}\frac{d^{n}}{dt^ {n}}\bigg{(}\frac{\partial\mathcal{L}}{\partial x^{(n)}}\bigg{)}=0 \tag{44}\]3  
Simplifying the rotational dynamics of quadrotor from Equation (10), we have:  
\[\left. \begin{array}{c}\ddot{\phi}=\frac{l}{l_{x}}U_{2}\\ \ddot{\theta}=\frac{l}{l_{y}}U_{3}\\ \ddot{\psi}=\frac{l}{l_{z}}U_{4}\end{array} \right\} \tag{51}\]  
For brevity, normalizing the inertial and length terms in Equation (51), we find a direct relation between the inputs and angular acceleration states. From Equations (48)-(51), we can write:  
\[m\left[ \begin{array}{c}x^{(iv)}\\ y^{(iv)}\\ z^{(iv)}\end{array}