# Control of an Omnidirectional UAV for Transportation and Manipulation Tasks
## 4 Motion Control
### Outer Loop: Position Controller  
The outer control loop is designed to track the assigned position by introducing a virtual control vector aimed at linearizing the position dynamics. In order to design the control law, it is worth rewriting Equation (14) as  
\[\mathbf{M}_{pp}\ddot{\mathbf{p}}_{0}+\mathbf{M}_{p\phi}\dot{\mathbf{\phi}}_{0}+\mathbf{C}_{p}\ddot{ \mathbf{p}}_{0}+\mathbf{g}_{p}=\mathbf{f}_{0}, \tag{17}\]  
where it has been assumed \(\mathbf{M}_{p\delta}\simeq\mathbf{O}_{3\times 2}\), with \(\mathbf{O}_{3\times 2}\) the (\(3\times 2\)) null matrix. This assumption allows to decouple the inner and the outer loop. Hence, on the basis of (17), the force to be exerted to the platform is computed as  
\[\mathbf{f}_{0}=\mathbf{M}_{pp}\mathbf{\gamma}_{p}+\mathbf{M}_{p\phi}\mathbf{\gamma}_{\phi}+\mathbf{C}_{ p}\dot{\mathbf{p}}_{0}+\mathbf{g}_{p^{\prime}} \tag{18}\]  
where the auxiliary inputs \(\mathbf{\gamma}_{*}\) (\(*=p,\phi\)) are defined as  
\[\mathbf{\gamma}_{p} = \ddot{\mathbf{p}}_{0,d}+\mathbf{K}_{D,p}\Big{(}\ddot{\mathbf{p}}_{0,d}-\dot{ \mathbf{p}}_{0}\Big{)}+\mathbf{K}_{P,p}\Big{(}\mathbf{p}_{0,d}-\mathbf{p}_{0}\Big{)}+\mathbf{K}_{ I,p}\int_{0}^{t}\Big{(}\mathbf{p}_{0,d}-\mathbf{p}_{0}\Big{)}d\rho, \tag{19}\] \[\mathbf{\gamma}_{\phi} = \ddot{\mathbf{\phi}}_{0,d}+\mathbf{K}_{D,\phi}\Big{(}\dot{\mathbf{\phi}}_{0,d} -\dot{\mathbf{\phi}}_{0}\Big{)}+\mathbf{K}_{P,\phi}\Big{(}\mathbf{\phi}_{0,d}-\mathbf{\phi}_{ 0}\Big{)}+\mathbf{K}_{I,\phi}\int_{0}^{t}\Big{(}\mathbf{\phi}_{0,d}-\mathbf{\phi}_{0}\Big{)} d\rho, \tag{20}\]  
and \(\mathbf{K}_{D,*}\), \(\mathbf{K}_{P,*}\), \(\mathbf{K}_{I,*}\) (\(*=p,\phi\)) are symmetric and positive definite gain matrices. **Remark 2**. : _The position control law is designed on the basis of (17) in lieu of (14), because (14) depends on \(\delta\), and thus computing \(\mathbf{f}_{0}\) would have required the reference values for \(\delta\) not available at this stage._  
Once the force \(\mathbf{f}_{0}\) is determined, it is possible to compute the reference values for the roll-pitch angles by exploiting (10) and the expression of \(\mathbf{R}_{1}^{0}\) and \(\mathbf{R}_{2}^{1}\). More in detail, let us express \(\mathbf{f}_{0}\) in the frame \(\mathbf{\Sigma}_{0}\), i.e.,  
\[\mathbf{f}_{0}^{0}=\mathbf{R}_{0}^{\mathrm{T}}\mathbf{f}_{0}. \tag{21}\]  
Then, in view of (10), it is  
\[\mathbf{f}_{0}^{0}=\mathbf{R}_{0}^{0}\mathbf{f}^{2}=\mathbf{R}_{1}^{0}\mathbf{R}_{2}^{1}\mathbf{f}^{2}. \tag{22}\]  
Figure 2: Block scheme of the overall control architecture, including the two loop controller, the interaction force estimator, and the propeller’s thrust mapping. By considering (1), the following relation can be derived  
\[ \begin{bmatrix}f_{0,x}^{0}\\ f_{0,y}^{0}\\ f_{0,z}^{0}\end{bmatrix} = \begin{bmatrix}0&-c_{\beta}&s_{\beta}\\ c_{\alpha}&-s_{\alpha}s_{\beta}&-c_{\beta}s_{\alpha}\\ s_{\alpha}&c_{\alpha}s_{\beta}&c_{\alpha}c_{\beta}\end{bmatrix}\begin{bmatrix} 0\\ 0\\ \beta_{z}\end{bmatrix} . \tag{23}\]  
Therefore, the desired values of the joints angles and the total thrust to be provided by the propellers are given by  
\[f_{z} = \left\|f_{0}^{0}\right\|=\left\|f_{0}\right\|, \tag{24}\] \[\alpha_{des} = \arctan\left(-\frac{f_{0,y}^{0}}{f_{0,z}^{0}}\right),\] (25) \[\beta_{des} = \arcsin\left(\frac{f_{0,x}^{0}}{\left\|f_{0}^{0}\right\|}\right). \tag{26}\]  
The desired velocity and acceleration of the roll-pitch joints can be derived by numerical differentiation; in practice, since \(\alpha_{des}\) and \(\beta_{des}\) are affected by noise, it is recommended to use suitable robust-to-noise filters. An effective approach is detailed in [35], where the time-varying filter proposed in [36] is adopted in order to compute both the first and second time-derivatives of the reference values. **Remark 3**. : _In view of (23), it could be noted that (25) and (26) are not well defined if_  
* \(\|f_{0}\|\) _vanishes: it can happen only if_ \(f_{z}=0\)_, namely in the presence of a null total thrust;_
* \(\alpha=\pm 90\) _deg or_ \(\beta=\pm 90\) _deg.