# Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method
## 3 Improved 3D-VO Method for UAV Collision Avoidance
### Formation of 3D-Velocity Obstacle Set  
For UAVs, 3D-VO [17] methods define the set of velocities that will collide with moving obstacles within the time horizon \(\tau\). The set is denoted by \(\mathcal{V}\mathcal{O}_{UAV|OBS}(v_{OBS})\):  
\[\mathcal{V}\mathcal{O}_{UAV|OBS}(\mathbf{v}_{OBS})=\{\mathbf{v}_{UAV}|\ \mathcal{R}(\mathbf{p}_{ UAV},\mathbf{v}_{UAV}-\mathbf{v}_{OBS})\cap(\mathcal{D}(\mathbf{p}_{UAV},r_{UAV})\oplus \mathcal{D}(\mathbf{p}_{OBS},r_{OBS}))\ \
eq\varnothing\ \} \tag{10}\]  
where \(\mathcal{R}(\mathbf{p},\mathbf{v})=\{\mathbf{p}+t\mathbf{v}|t>0\}\) includes the rays with the starting point \(\mathbf{p}\) and the direction \(\mathbf{v}\). The UAV will not collide with the moving obstacle with velocity \(\mathbf{v}_{OBS}\) when \(\mathbf{v}_{UAV}\) lies outside \(\mathcal{V}\mathcal{O}_{UAV|OBS}(\mathbf{v}_{OBS})\). The geometry of \(\mathcal{V}\mathcal{O}_{UAV|OBS}(\mathbf{v}_{OBS})\) is a cone. Related parameters are shown in Equation (11). \[\left\{ \begin{array}{l}O_{vo}=\mathbf{v}_{OBS},\,d_{ai}=\sqrt{d_{uo}^{2}-r_{ps}^{ 2}}\\ d_{vo}=\frac{d_{ai}^{2}}{du_{ua}},\,r_{vo}=r_{ps}\frac{d_{ai}}{du_{uo}}\\ \eta_{vo}=\arcsin\Big{(}\frac{r_{uo}}{d_{ai}}\Big{)}\\ \mathbf{k}_{vo}=\left[\begin{array}{c}\cos\theta_{uo}\cos\psi_{uo}\\ \cos\theta_{uo}\sin\psi_{uo}\\ \sin\theta_{uo}\end{array}\right]d_{vo}\end{array} \right. \tag{11}\]  
where \(d_{uo}\) is the distance between the UAV and the moving obstacle, \(\mathbf{k}_{vo}\) and \(\eta_{VO}\) represent the axis and the half angle of the cone, respectively, as shown in Figure 2a. The position of the obstacle is described by the spherical coordinates relative to frame-\(n\). The vertex \(\mathcal{O}_{\mathbf{VO}}\) of the cone is translated from the position \(O_{vo}^{*}\) of the UAV along \(\mathbf{v}_{OBS}\), as shown in Figure 2b. A standard cone parametric equation is illustrated in Equation (12)  
\[\left[ \begin{array}{c}x_{c}\\ y_{c}\\ z_{c}\end{array} \right]=\left[ \begin{array}{c}q\\ q\tan\eta_{vo}\cos\varphi\\ q\tan\eta_{vo}\sin\varphi\end{array} \right] \tag{12}\]  
where \(q\in[0,d_{VO}]\), \(\varphi\in[0,2\pi]\). Further, it is necessary to rotate the above cone twice to align its cone axis with \(\mathbf{k}_{vo}\) and translate along \(\mathbf{v}_{OBS}\) until the vertex coincides with \(\mathcal{O}_{vo}\). Finally, we obtain the geometry of \(\mathcal{V}\mathcal{O}_{UAV|OBS}(\mathbf{v}_{OBS})\), which is called the VO cone. This process is represented by Equation (13). When the distance \(d_{uo}\) between the UAV and the obstacle below the safety threshold distance \(d_{safe}(d_{safe}>r_{UAV}+r_{OBS})\), \(\mathcal{V}\mathcal{O}_{UAV|OBS}(\mathbf{v}_{OBS})\) is updated in real time to determine  
Figure 2: (**a**) Geometric elements of the VO cone. (**b**) The VO cone relative to different coordinate systems. whether the collision will happen in the future. This is mainly based on whether the positive extension of \(v_{UAV}\) is inside the VO cone. \[\left[ \begin{array}{c}x_{VO}\\ y_{VO}\\ z_{VO}\end{array} \right]=\left[ \begin{array}{ccc}\cos\psi_{UIO}&\sin\psi_{UIO} &0\\ -\sin\psi_{UIO}&\cos\psi_{UIO}&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{ccc}\cos\theta_{UIO}&0&-\sin \theta_{UIO}\\ 0&1&0\\ \sin\theta_{UIO}&0&\cos\theta_{UIO}\end{array} \right]\left[ \begin{array}{c}x _{c}\\ y_{c}\\ z_{c}\end{array} \right]+O_{vo} \tag{13}\]  
\[\left\{ \begin{array}{c}d_{UIO}\leq d_{safe}\\ [v_{UAV}-O_{vo}]^{\prime}k_{vo}>\cos\eta_{vo}\|v_{UAV}-O_{vo}\|\end{array} \right. \tag{14}\]  
If the condition in Equation (14) is satisfied, the positive extension of \(v_{UAV}\) is inside the VO cone, then the set of avoidance velocities further generate \(\mathcal{AV}_{UAV|OBS}=W\backslash\mathcal{VO}_{UAV|OBS}(v_{OBS})\). When the avoidance velocity \(v_{avo}\) of the UAV in the next time horizon belongs to \(\mathcal{AV}_{UAV|OBS}\), the collision can be avoided. The solution of \(v_{avo}\) is introduced in Section 4.2. The termination of obstacle avoidance mode cannot be judged by \(v_{UAV}\
otin\mathcal{VO}_{UAV|OBS}(v_{OBS})\) alone.