## 3 Motion Planning
### Trajectory Generation
\right]=\left[\begin{array}{c}\left(-\dot{\theta}\sin\psi \dot{\psi}-\theta\cos\psi\dot{\psi}^{2}+\dot{\phi}\cos\psi\dot{\psi}-\phi\sin \psi\dot{\psi}^{2}\right)U_{1}+2\Big{(}\dot{\theta}\cos\psi-\theta\sin\psi\dot {\psi}+\dot{\phi}\sin\psi+\phi\cos\psi\dot{\psi}\dot{\psi}\Big{)}\dot{U}_{1}\\ \Big{(}\dot{\theta}\cos\psi\dot{\psi}-\theta\sin\psi\dot{\psi}^{2}+\dot{\phi} \sin\psi\dot{\psi}+\phi\cos\psi\dot{\psi}^{2}\Big{)}U_{1}+2\Big{(}\dot{\theta }\sin\psi+\theta\cos\psi\dot{\psi}-\dot{\phi}\cos\psi+\phi\sin\psi\dot{\psi} \dot{\psi}\dot{U}_{1}\] \[\qquad\qquad\qquad\qquad\qquad\qquad\left(2\Big{(}\dot{\phi} \dot{\theta}\sin\phi\sin\theta\Big{)}-\Big{(}\dot{\phi}^{2}+\dot{\theta}^{2} \Big{)}\cos\phi\cos\theta\Big{)}U_{1}-2\Big{(}\dot{\theta}\cos\phi\cos\theta +\dot{\phi}\sin\phi\cos\theta\Big{)}\dot{U}_{1} \tag{52}\]  
The translational states of the quadrotor create a fourth-order system; therefore, the control input directly relates to the snap of the position states. This completes the proof of Theorem 1. \(\square\)  
In order to achieve minimum control effort, we must minimize the snap of the translational states. To attain the goal just mentioned, the optimal trajectory \(\chi(t)\) will assume the following form:  
\[\chi(t)=argmin_{(x(t))}\int\limits_{0}^{T}\mathcal{L}\Big{(}x^{(iv)},x^{(iii)},\ddot{x},\dot{x},x,t\Big{)}\cdot dt \tag{53}\]  
where \(\mathcal{L}\Big{(}x^{(iv)},x^{(iii)},\ddot{x},\dot{x},x,t\Big{)}\cdot dt\) is equal to \(\Big{(}x^{(iv)}\Big{)}^{2}\cdot dt\) for minimum snap trajectory. Now the Euler--Lagrange equation can be solved as:  
\[\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{)}-\frac{d^{3}}{dt^{3}}\bigg{(}\frac{\partial\mathcal{L} }{\partial x^{(3)}}\bigg{)}+\frac{d^{4}}{dt^{4}}\bigg{(}\frac{\partial \mathcal{L}}{\partial x^{(4)}}\bigg{)}=0 \tag{54}\]  
This will yield the following condition:  
\[x^{(viii)}=0 \tag{55}\]  
The solution of Equation (55) yields the optimal trajectory of the following form:  
\[\chi(t)=\lambda_{0}+\lambda_{1}t+\lambda_{2}t^{2}+\cdots+\lambda_{m}t^{m}\text { where }m=7 \tag{56}\]  
The trajectory computed above is one-dimensional and serves only for one segment. However, for a quadrotor UAV, to perform an autonomous flight through obstacles, the dimensionality of the trajectory is four and includes \(x(t),\ y(t),\ z(t),\psi(t)\). Moreover, for a shortest path search problem, there are usually \(n\) trajectory segments for \(n+1\) number of waypoints, including start and goal points. We can formulate a trajectory for multiple dimensions and multiple segments as:  
\[p_{i}(t)_{x(t),y(t),z(t),\psi(t)}=\lambda_{i0}+\lambda_{i1}t+\lambda_{i2}t^{2 }+\cdots+\lambda_{im}t^{m}\ \forall\ i=0\cdots n\text{ and }m=7 \tag{57}\]  
\[p_{i}(t)_{x(t),y(t),z(t),\psi(t)}=\sum\limits_{j=0}^{m}\lambda_{ij}t^{j} \tag{58}\]  
Using Equation (57), a complete trajectory can be formulated over various time slots as follows:  
\[P(t)_{x(t),y(t),z(t),\psi(t)}= \begin{cases}\sum\limits_{j=0}^{m}\lambda_{0}t^ {j}&t_{0}\leq t<t_{1}\\ \sum\limits_{j=0}^{m}\lambda_{1j}t^{j}&t_{1}\leq t<t_{2}\\ \vdots&\\ \sum\limits_{j=0}^{m}\lambda_{nj}t^{j}&t_{n}\leq t<t_{n+1}\end{cases}  \tag{59}\]  
where \(m\) defines the degree of polynomial. To solve the coefficients of Equation (59), some constraints must inevitably be considered. These constraints are explained subsequently.