## 2 Quadrotor UAV: Modeling and Control
### Strategy Control of Trajectory Tracking
#### 2.2.1 PD Controller  
The dynamics shown in (3) can be represented as a second-order system, in the form  
\[\ddot{x}=f(\chi)+g(\chi)u, \tag{4}\]  
Figure 3: Proposed control structure to trajectory tracking.  
Figure 2: Order of rotations from \(W\) to \(B\).  
where \(f(\chi)\), in this work, it is a nonlinear function due to the drag forces of the QUAV, \(\chi=[\chi,\ \dot{\chi}]\), \(g(\chi)\
eq 0\) is a known function \(u\), is the control signal. Control law is designed as  
\[u=\frac{1}{g(\chi)}\Big{(}-f(\chi)+\mathbf{K}^{T}\mathbf{E}+\breve{\chi}_{d}\Big{)}, \tag{5}\]  
where, \(\breve{\chi}_{d}\) is the second derivative of the desired trajectory, \(\mathbf{E}=[e\ e]^{T}\), with \(e=\chi_{d}-\chi\) substituting (5) in (4), the closed-loop dynamics of the system are  
\[\breve{e}+k_{p}e+k_{d}\dot{e}=0,\]
* Translational and Altitude Control The altitude dynamics shown in (3), can be represented as (4), \[\breve{z}=\frac{1}{m}(\cos\phi\cos\theta)u_{1}-g-\frac{k_{3}}{m}\dot{z},\] (6) Thus, if the control signal is designed as in (5) we get \[u_{1}=\frac{m}{\cos\theta\cos\phi}\left(\frac{k_{3}}{m}\dot{z}+\mathbf{K}_{z}^{T} \mathbf{E}_{z}+\breve{z}_{d}\right)+g,\] (7) Equation (7) is constrained \((-\pi/2<\phi<\pi/2(\ \text{and}\ (-\pi/2<\theta<\pi/2)\).  
For translation dynamic in the axis \(X\), a virtual control signal is selected as \(\mu_{x}=\cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi\), then becomes as  
\[m\breve{x}=f(\mathbf{x})+\mu_{x}u_{1}, \tag{8}\]  
and the control signal is chosen as in (5)  
\[\mu_{x}=\frac{m}{u_{1}}\bigg{(}\frac{k_{1}}{m}\dot{x}+\mathbf{K}_{x}^{T}\mathbf{E}_{x} +\breve{x}_{d}\bigg{)}. \tag{9}\]  
Similarly, for the dynamic on the \(Y\) axis, \(\mu_{y}=\cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi\), we get  
\[m\dot{y}=\frac{k_{2}}{m}\dot{y}+\mu_{y}u_{1}, \tag{10}\]  
with the control signal as form as (5), therefore  
\[\mu_{y}=\frac{m}{u_{1}}\bigg{(}\frac{k_{2}}{m}\dot{y}+\mathbf{K}_{y}^{T}\mathbf{E}_{y }+\breve{y}_{d}\bigg{)}. \tag{11}\]  
Equations (9) and (11) are true for \(u_{1}>0\), which is physically correct because \(u_{1}\) represents the lift force generated by the rotors of the vehicle.  
Virtual control signals \(\mu_{x}\) y \(\mu_{y}\) allow to calculate desired angles for attitude controller _roll_, \(\phi_{d}\) y en _pitch_, \(\theta_{d}\):  
\[\begin{array}{c}\left[\begin{array}{c}\cos(\phi)\sin(\theta)\\ \sin(\phi)\end{array}\right]=\left[\begin{array}{cc}\cos(\psi)&\sin(\psi)\\ \sin(\psi)&-\cos(\psi)\end{array}\right]\left[\begin{array}{c}\mu_{x}\\ \mu_{y}\end{array}\right]\\ \phi_{d}=asin(\sin(\psi)\mu_{x}-\cos(\psi)\mu_{y})\\ \theta_{d}=asin\bigg{(}\frac{\cos(\psi)\mu_{x}+\sin(\psi)\mu_{y}}{\cos(\phi)} \bigg{)}.\end{array} \tag{12}\]
* Attitude Controller For the orientation dynamics, they can be represented as (4) and the control signal can be designed as in (5). For the movement of _roll_, we have the control signal \(u_{2}\) as:\[u_{2}=I_{x}\left(\frac{k_{4}}{I_{x}}\dot{\phi}^{2}-\frac{(Iy-Iz)}{Ix}\dot{\phi} \dot{\psi}+\mathbf{K}_{\phi}^{T}\mathbf{E}_{\phi}+\ddot{\phi}_{d}\right), \tag{13}\]  
For the movement of _pitch_, we have the control signal \(u_{3}\) as:  
\[u_{3}=I_{y}\left(\frac{k_{5}}{I_{y}}\dot{\phi}^{2}-\frac{(Iz-Ix)}{Iy}\dot{\phi} \dot{\psi}+\mathbf{K}_{\theta}^{T}\mathbf{E}_{\theta}+\ddot{\theta}_{d}\right), \tag{14}\]  
Finally, for the movement of _yaw_, we have the control signal \(u_{4}\) as:  
\[u_{4}=I_{z}\left(\frac{k_{6}}{I_{z}}\dot{\psi}^{2}-\frac{(Ix-Iy)}{Iz}\dot{\phi }\dot{\theta}+\mathbf{K}_{\psi}^{T}\mathbf{E}_{\psi}+\ddot{\psi}_{d}\right). \tag{15}\]