# Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method
## 5 Attitude and Velocity Control Commands
### Attitude Controller  
In this section, we implement a sliding mode attitude controller to align the \(s\)-frame of the UAV with the \(d\)-frame. We do so firstly by defining the rotational quaternion between the \(s\)-frame \(\mathbf{\quad}\) and \(\mathbf{  
to \(\mathbf{q}_{ds}\to\left[\begin{array}{cc}1&\mathbf{0}\end{array}\right]^{\mathrm{T}}\). Then, taking the derivative of \(\mathbf{q}_{ds}^{*}\) gives \(\dot{\mathbf{q}}_{ds}^{*}=D_{q}\big{(}\mathbf{q}_{ds}^{*}\big{)}\mathbf{w}_{ds}^{s}\), where \(D_{q}\big{(}\mathbf{q}_{ds}^{*}\big{)}:=\frac{1}{2}\bigg{[}\begin{array}{c} \varepsilon_{ds}^{\mathrm{T}}\\ \eta_{ds}I+S(\varepsilon_{ds})\end{array}\bigg{]}\). Finally, we define the sliding variable [29; 30]:  
\[\mathbf{s}=\mathbf{w}_{nb}^{b}-\mathbf{w}_{ref}^{b} \tag{46}\] \[\mathbf{w}_{ref}^{b}=\mathbf{w}_{nd}^{b}-\mathbf{w}_{bs}^{b}-\kappa_{1}R_{s}^ {b}D_{q}^{\mathrm{T}}\mathbf{q}_{ds}^{*} \tag{47}\]  
where \(\mathbf{w}_{ref}^{b}\) is the projection of the reference angular velocity relative to \(n\)-frame on the \(b\)-frame.  
\[\dot{\mathbf{s}}=\frac{1}{\gamma}\Big{(}M_{at}^{b}\Big{(}\mathbf{w}_{nb}^{b},\mathbf{v}_{ UAV}^{b}\Big{)}-S\Big{(}\mathbf{w}_{nb}^{b}\Big{)}J\mathbf{w}_{nb}^{b}+A\mathbf{u}_{2} \Big{)}-\dot{\mathbf{w}}_{ref}^{b} \tag{48}\]  
We introduce a Lyapunov function \(V_{3}=\frac{1}{2}\kappa_{2}\big{(}\mathbf{q}_{ds}^{*}\big{)}^{\mathrm{T}}\mathbf{q}_ {ds}+\frac{1}{2}\mathbf{s}^{\mathrm{T}}J\mathbf{s}\) and take the derivative of \(V_{3}\), giving  
\[\dot{V}_{3}=\kappa_{2}(\mathbf{q}_{ds}^{*})^{\mathrm{T}}D_{q}\mathbf{w}_{ds}^{s}+\mathbf{ s}^{\mathrm{T}}\Big{(}M_{at}^{b}\Big{(}\mathbf{w}_{nb}^{b},\mathbf{v}_{UAV}^{b}\Big{)}-S \Big{(}\mathbf{w}_{nb}^{b}\Big{)}J\mathbf{w}_{nb}^{b}+A\mathbf{u}_{2}-J\dot{\mathbf{w}}_{ref}^ {b}\Big{)} \tag{49}\]  
where \(\kappa_{2}>0\), and since \(\mathbf{w}_{ds}^{s}=R_{b}^{s}\Big{(}\mathbf{s}-\kappa_{1}R_{s}^{b}D_{q}^{\mathrm{T}} \mathbf{q}_{ds}^{*}\Big{)}\), \(\dot{V}_{3}\) can be rewritten as  
\[\dot{V}_{3}=-\kappa_{1}\kappa_{2}(\mathbf{q}_{ds}^{*})^{\mathrm{T}}D_{q}D_{q}^{ \mathrm{T}}\mathbf{q}_{ds}^{*}+\mathbf{s}^{\mathrm{T}}\Big{(}M_{at}^{b}\Big{(}\mathbf{w}_{ nb}^{b},\mathbf{v}_{UAV}^{b}\Big{)}-S\Big{(}\mathbf{w}_{nb}^{b}\Big{)}J\mathbf{w}_{nb}^{b}+A \mathbf{u}_{2}-J\dot{\mathbf{w}}_{ref}^{b}+\kappa_{2}R_{s}^{b}D_{q}^{\mathrm{T}}\mathbf{q}_ {ds}^{*}\Big{)} \tag{50}\]  
The control input \(\mathbf{u}_{2}\) can be given similar to before as  
\[\mathbf{u}_{2}=A^{-1}\Big{(}S\Big{(}\mathbf{w}_{nb}^{b}\Big{)}J\mathbf{w}_{nb}^{b}-M_{at}^ {b}\Big{(}\mathbf{w}_{nb}^{b},\mathbf{v}_{UAV}^{b}\Big{)}+J\dot{\mathbf{w}}_{ref}^{b}- \kappa_{2}R_{s}^{b}D_{q}^{\mathrm{T}}\mathbf{q}_{ds}^{*}-K_{s}\mathbf{s}\Big{)} \tag{51}\]  
where \(K_{s}\) is a positive-definite matrix. Substituting Equation (51) into Equation (50) gives  
\[\dot{V}_{3}=-\kappa_{1}\kappa_{2}(\mathbf{q}_{ds}^{*})^{\mathrm{T}}D_{q}D_{q}^{ \mathrm{T}}\mathbf{q}_{ds}^{*}-\mathbf{s}^{\mathrm{T}}K_{s}\mathbf{s}\leq\frac{-\kappa_{1 }\kappa_{2}}{8}\|\mathbf{q}_{ds}^{*}\|^{2}-\tilde{\mathbf{\ }}_{\min}(K_{s})\|\mathbf{s}\|^{2} \tag{52}\]  
where \(\tilde{\mathbf{\ }}_{\min}\) is the minimal eigenvalue of the matrix \(K_{s}\). According to the Lyapunov stability theory and Equation (52), we find \(\big{(}\mathbf{q}_{ds}^{*},\mathbf{s}\big{)}\to(0,0)\); then, as a consequence, the \(s\)-frame coincides with the \(d\)-frame.  
A flowchart of the overall method is shown in Figure 8. Algorithm 1 summarizes the collision avoidance guidance frame, where \(\mathcal{V}_{k}\) is the set of the avoidance velocity at the \(K\)-th step, and the function feasible Section(\(\delta\)) checks if the shape of the conic section is expected, given a rotation angle \(\delta\) in Equation (18). If Equation (19) satisfied, then return 'true'; else 'false'. We assumed that the control system as well as the detection system operate at the same rate and uniformly set the cycle as the time constant \(\tau\).  
Figure 8: Collision avoidance flow chart.