# Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method
## 4 Multiple Task Implementation Using a Hierarchical Architecture
### Task Quaternion 1: Destination Point Tracking  
To guide the UAV to the destination point, the position error of the UAV is projected onto the \(\mathbf{x}^{\prime}\) axis of the error coordinate system. Then, the UAV translates along with \(\mathbf{x}^{\prime}\) under the action of the attitude controller until the destination point is reached. As in Figure 7, \(e_{ud}^{\mu}\) denotes the position error between \(\mathbf{p}_{d}^{\mu}\) and \(\mathbf{p}_{ULAV}^{n}\) in the \(n\)-frame. The positive direction of the \(x\)-axis of the \(e\)-frame is aligned with \(e_{ud}^{n}\), so the \(e\)-frame can be defined through the rotation:  
\[\mathbf{e}_{ud}^{e}=\left[\begin{array}{cc}\|\mathbf{e}_{ud}^{n}\|\quad 0\quad 0\end{array} \right]^{\mathrm{T}}=\mathbf{R}_{\mathbf{n}}^{e}(\mathbf{p}_{d}^{n}-\mathbf{p}_{ULAV}^{n})=\bm {R}_{\mathbf{n}}^{e}e_{ud}^{n} \tag{26}\]  
The control objective is to make \(\mathbf{e}^{e}\rightarrow\left[\mathsf{0}^{\mathrm{T}}\right]^{\mathrm{T}}\). The rotation quaternion between the \(n\)-frame and the \(e\)-frame and the rotation matrix can be defined as  
\[\theta_{ne}=\cos^{-1}\left(\frac{\mathbf{e}_{ud}^{e}\cdot\mathbf{e}_{ud}^{n}}{\|\mathbf{e }_{ud}^{n}\|}\right).\mathbf{k}_{\mathbf{n}e}=\frac{\mathbf{e}_{ud}^{e}\times\mathbf{e}_{ud}^{ n}}{\|\mathbf{e}_{ud}^{e}\times\mathbf{e}_{ud}^{n}\|} \tag{27}\]  
\[\mathbf{q}_{ne}=\left[\begin{array}{cc}\eta_{ne}&\mathbf{e}_{ne}^{\mathrm{T}}\end{array} \right]^{\mathrm{T}}=\left[\begin{array}{cc}\cos(\frac{\theta_{ge}}{2})&\mathbf{ k}_{ne}^{\mathrm{T}}\sin(\frac{\theta_{ge}}{2})\end{array}\right]^{\mathrm{T}} \tag{28}\]  
\[\mathbf{R}_{e}^{n}=\mathrm{I}+2\eta_{ne}S(\mathbf{e}_{\mathbf{n}e})+2S^{2}(\mathbf{e}_{\mathbf{n}e}) \tag{29}\]  
Figure 6: The hierarchical architecture.  
Figure 7: Position vectors in the relative three-dimensional space.  
To obtain the desired angular velocity, we first take the derivative of Equation (26), which gives  
\[\dot{\mathbf{e}}^{e}=-S(\mathbf{w}_{ne}^{e})\mathbf{e}^{e}-R_{b}^{e}\mathbf{v}_{UAV}^{b} \tag{30}\]  
Then, substituting equation \(-S(\mathbf{w}_{ne}^{e})\mathbf{e}^{e}=S(\mathbf{e}^{e})\mathbf{w}_{ne}^{e}\) into Equation (30), gives  
\[\left[\begin{array}{c}\left\|\dot{\mathbf{e}}_{nd}^{n}\right\|\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}0\\ \left\|\mathbf{e}_{nd}^{n}\right\|\mathbf{w}_{ne}^{e}(2)\\ \left\|\mathbf{e}_{nd}^{n}\right\|\mathbf{w}_{ne}^{e}(3)\end{array}\right]-R_{b}^{e} \mathbf{v}_{UAV}^{b} \tag{31}\]  
It can be seen from Equation (31) that \(\mathbf{w}_{ne}^{e}\) only contains the second and third terms. Therefore, rewriting Equation (30) as Equation (32) will not affect the final calculation result of \(\mathbf{w}_{ne}^{e}\).  
\[S(\mathbf{e}_{nt}^{e})\mathbf{w}_{ne}^{e}=R_{b}^{e}\mathbf{v}_{UAV}^{b} \tag{32}\]  
\[\mathbf{w}_{ne}^{e}=S^{\dagger}(\mathbf{e}_{nd}^{e})\mathbf{R}_{b}^{n}\mathbf{R}_{n}^{e}\mathbf{v} _{UAV}^{b} \tag{33}\]  
Equation (28) defines the rotation between two coordinate systems. When the task quaternion in Equation (28) degenerates into a unit quaternion, we think that two coordinate systems have reached a coincidence and the task is completed. At the same time, the desired angular velocity in Equation (33) converges to zero.