# Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method

^{*}

## Abstract

**:**

## Featured Application

**This work provides a generalization of the velocity obstacle (VO) collision avoidance strategy to nonlinear second-order underactuated systems in three-dimensional dynamic uncertain environments.**

## Abstract

## 1. Introduction

#### 1.1. Related Works

#### 1.2. Contributions

#### 1.3. Organization

## 2. Problem Formulation

#### 2.1. Notation

**b**relative to vector

**c**on

**b**.

**Theorem**

**1.**

**([27]).**If a rigid body makes a finite number of consecutive rotations around the axes that pass the same fixed point, then the size of the composite rotation angle has nothing to do with the order of each continuous rotation, but the direction of the composite rotation is related to the order of each continuous rotation.

#### 2.2. UAV Dynamics Model

**J**is the inertia matrix, ${M}_{at}^{b}$ is the resultant moment of the aerodynamic and thrust moments of the UAV, which is a nonlinear function of ${w}_{nb}^{b},{v}_{UAV}^{b}$, and ${\mathit{u}}_{2}={\left[\begin{array}{ccc}{\varpi}_{a}& {\varpi}_{e}& {\varpi}_{r}\end{array}\right]}^{\mathrm{{\rm T}}}$ is the control input of the rotation loop, denoting the rudder angle.

#### 2.3. Problem Statement

- 1.
- The UAV and obstacle are seen as spheres with radii ${r}_{UAV},\text{}{r}_{OBS}$, respectively;
- 2.
- The obstacle mentioned in this article is assumed to be a moving but non-maneuvering obstacle;
- 3.
- Suppose sensors mounted on the UAV can accurately provide measurements, such as locations ${\mathit{p}}_{UAV},\text{}{\mathit{p}}_{OBS}\in {\mathbb{R}}^{3}$, velocities ${v}_{UAV},\text{}{v}_{OBS}\in {\mathbb{R}}^{3}$ and radii ${r}_{UAV},\text{}{r}_{OBS}$ of the UAV itself and obstacles;
- 4.
- The control system cycle is consistent with the detection system cycle;
- 5.
- The influence of wind is ignored during the flight of the UAV to simplify the problem.

**p**with radius $r$. Similarly, the airspace occupied by the UAV can be expressed as $O\left({\mathit{p}}_{\mathrm{UAV}}\right)=D\left({\mathit{p}}_{UAV},{r}_{UAV}\right)\in {\mathbb{R}}^{3}$, where ${r}_{UAV}$ is the radius of the UAV. This also applies to obstacles $O\left({\mathit{p}}_{OBS}\right)=D\left({\mathit{p}}_{OBS},\text{}{r}_{OBS}\right)\in {\mathbb{R}}^{3}$.

**Definition**

**1.**

**Definition**

**2.**

## 3. Improved 3D-VO Method for UAV Collision Avoidance

#### 3.1. Formation of 3D-Velocity Obstacle Set

#### 3.2. Avoidance Planes

#### 3.3. Avoidance Velocity

## 4. Multiple Task Implementation Using a Hierarchical Architecture

#### 4.1. Task Quaternion 1: Destination Point Tracking

#### 4.2. Task Quaternion 2: Collision Avoidance

## 5. Attitude and Velocity Control Commands

#### 5.1. Velocity Controller

#### 5.2. Attitude Controller

Algorithm 1. Attidude and Velocity Commands at the K-th Step |

1. design parameter: ${r}_{\mathit{ps}}\text{},\text{}{d}_{safe}\text{},\text{}{\alpha}_{\mathrm{max}}\text{},\text{}{\mathit{p}}_{d}^{n},\tau ,\mathcal{A}\mathcal{G}=[0\xb0,\pm \frac{\pi}{4},\frac{\pi}{2}]$ |

2. Input: ${\mathit{p}}_{UAV}^{n}\text{},\text{}{\mathit{p}}_{OBS}^{n}\text{},\text{}{\mathit{v}}_{UAV}\text{},\text{}{\mathit{v}}_{OBS}\text{},\text{}{r}_{OBS}$ |

3. Output: ${\mathit{u}}_{2}\text{},\text{}{T}_{x}^{b}$ |

4. begin |

5. obtain ${\mathit{q}}_{ne}\left(k\right),{\mathit{w}}_{ne}^{e}\left(k\right)$ by substituting ${\mathit{p}}_{UAV}^{n}\left(k\right),\text{}{\mathit{p}}_{d}^{n}$ into Equations (26)–(33) |

6. if $\Vert {\mathit{p}}_{UAV}^{n}-{\mathit{p}}_{OBS}^{n}{\Vert}_{2}<{d}_{safe}$ and meet the conditions: Equation (15) then |

7. for each δ in $\mathcal{A}\mathcal{G}$, do |

8. ${\mathit{v}}_{avo}^{\delta}\leftarrow $ Solution of quadratic Equation (23) with constrain Equation (22) |

9. if feasible Section(δ) = ’Ture’ |

10. ${\mathcal{V}}_{k}\leftarrow {\mathcal{V}}_{k}\text{}\cup \text{}{v}_{avo}^{\delta}$ |

11. end if |

12. end for |

13. if ${\mathcal{V}}_{k}\ne \varnothing $ then |

14. select ${\mathit{v}}_{avo}\left(k\right)\in {\mathcal{V}}_{k}$ with the smallest rotation angle α |

15. return ${\mathit{v}}_{avo}\left(k\right)$ |

16. // If ${\mathcal{V}}_{k}=\varnothing $, the $\mathcal{A}\mathcal{G}$ needs to be extended |

17. end |

18. Map ${\mathit{v}}_{avo}\left(k\right)$ to ${\mathit{q}}_{ea}\left(k\right)\text{},\text{}{\mathit{w}}_{ea}^{a}\left(k\right)$ following Equation (25), Equations (34)–(38) |

${\mathit{q}}_{nd}\left(k\right)$←Equation (39) |

19. else |

20. ${\mathit{q}}_{ea}\left(k\right)\leftarrow {\left[\begin{array}{cc}1& \mathbf{0}\end{array}\right]}^{\mathrm{{\rm T}}}\text{};\text{}{\mathit{w}}_{ea}^{a}\left(k\right)\leftarrow {\left[\mathbf{0}\right]}^{\mathrm{{\rm T}}};{\mathit{q}}_{nd}\left(k\right)\leftarrow $ Equation (39) |

21. end if |

22. Compute velocity command ${T}_{x}^{b}\left(k\right)$ following Equations (40)–(44) |

23. Compute Attitude command ${\mathit{u}}_{2}\left(k\right)$ following Equations (46)–(51) |

24. Apply controls |

25. end |

## 6. Simulation

## 7. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Conflicts of Interest

## References

- Zhu, X.; Yan, B.; Yue, Y. Path planning and collision avoidance in unknown environments for usvs based on an improved d* Lite. Appl. Sci.
**2021**, 11, 7863. [Google Scholar] [CrossRef] - Oland, E.; Andersen, T.S.; Kristiansen, R. Subsumption architecture applied to flight control using composite rotations. Automatica
**2016**, 69, 195–200. [Google Scholar] [CrossRef] [Green Version] - Moe, S.; Antonelli, G.; Pettersen, K.Y.; Schrimpf, J. Experimental results for set-based control within the singularity-robust multiple task-priority inverse kinematics framework. In Proceedings of the 2015 IEEE International Conference on Robotics and Biomimetics, Zhuhai, China, 6–9 December 2015; pp. 1233–1239. [Google Scholar]
- Moe, S.; Teel, A.R.; Antonelli, G.; Pettersen, K.Y. Stability analysis for set-based control within the singularity-robust multiple task-priority inverse kinematics framework. In Proceedings of the 2015 54th IEEE Conference on Decision and Control, Osaka, Japan, 15–18 December 2015; pp. 171–178. [Google Scholar]
- Lin, Y.; Saripalli, S. Sampling-Based Path Planning for UAV Collision Avoidance. IEEE Trans. Intell. Transp. Syst.
**2017**, 18, 3179–3192. [Google Scholar] [CrossRef] - Fan, Y.; Sun, X.; Wang, G.; Mu, D. Collision Avoidance Controller for Unmanned Surface Vehicle Based on Improved Cuckoo Search Algorithm. Appl. Sci.
**2021**, 11, 9741. [Google Scholar] [CrossRef] - Fiorini, P.; Shiller, Z. Motion planning in dynamic environments using velocity obstacles. Int. J. Robot. Res.
**1998**, 17, 760–772. [Google Scholar] [CrossRef] - Park, S.O.; Lee, M.C.; Kim, J. Trajectory Planning with Collision Avoidance for Redundant Robots Using Jacobian and Artificial Potential Field-based Real-time Inverse Kinematics. Int. J. Control Autom. Syst.
**2020**, 18, 2095–2107. [Google Scholar] [CrossRef] - Zhang, J.; Yan, J.; Zhang, P. Fixed-wing UAV formation control design with collision avoidance based on an improved artificial potential field. IEEE Access
**2018**, 6, 78342–78351. [Google Scholar] [CrossRef] - Choi, D.; Kim, D.; Lee, K. Enhanced potential field-based collision avoidance in cluttered three-dimensional urban environments. Appl. Sci.
**2021**, 11, 11003. [Google Scholar] [CrossRef] - Jenie, Y.I.; Van Kampen, E.J.; De Visser, C.C.; Ellerbroek, J.; Hoekstra, J.M. Selective velocity obstacle method for deconflicting maneuvers applied to unmanned aerial vehicles. J. Guid. Control Dyn.
**2015**, 38, 1140–1145. [Google Scholar] [CrossRef] - Bareiss, D.; Van Den Berg, J. Generalized reciprocal collision avoidance. Int. J. Robot. Res.
**2015**, 34, 1501–1514. [Google Scholar] [CrossRef] - Alonso-Mora, J.; Beardsley, P.; Siegwart, R. Cooperative Collision Avoidance for Nonholonomic Robots. IEEE Trans. Robot.
**2018**, 34, 404–420. [Google Scholar] [CrossRef] [Green Version] - Wilkie, D.; Van Den Berg, J.; Manocha, D. Generalized velocity obstacles. In Proceedings of the 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, USA, 11–15 October 2009; pp. 5573–5578. [Google Scholar]
- Alonso-mora, J.; Breitenmoser, A.; Rufli, M.; Beardsley, P.; Siegwart, R. Optimal Reciprocal Collision Avoidance for Multiple Non-Holonomic Robots; Springer: Berlin/Heidelberg, Germany, 2013; Volume 83, pp. 203–216. [Google Scholar]
- Chakravarthy, A.; Ghose, D. Generalization of the collision cone approach for motion safety in 3-D environments. Auton. Robots
**2012**, 32, 243–266. [Google Scholar] [CrossRef] - Jenie, Y.I.; Van Kampen, E.J.; De Visser, C.C.; Ellerbroek, J.; Hoekstra, J.M. Three-dimensional velocity obstacle method for uncoordinated avoidance maneuvers of unmanned aerial vehicles. J. Guid. Control Dyn.
**2016**, 39, 2312–2323. [Google Scholar] [CrossRef] [Green Version] - Tan, C.Y.; Huang, S.; Tan, K.K.; Teo, R.S.H. Three Dimensional Collision Avoidance for Multi Unmanned Aerial Vehicles Using Velocity Obstacle. J. Intell. Robot. Syst. Theory Appl.
**2020**, 97, 227–248. [Google Scholar] [CrossRef] - Van Den Berg, J.; Snape, J.; Guy, S.J.; Manocha, D. Reciprocal collision avoidance with acceleration-velocity obstacles. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, China, 9–13 May 2011; pp. 3475–3482. [Google Scholar]
- Rufli, M.; Alonso-Mora, J.; Siegwart, R. Recipro collision avoidance with motion continuity constraints. IEEE Trans. Robot.
**2013**, 29, 899–912. [Google Scholar] [CrossRef] - Ma, Y.; Manocha, D.; Wang, W. AutoRVO: Local Navigation with Dynamic Constraints in Dense Heterogeneous Traffic. arXiv
**2018**, arXiv:1804.02915. [Google Scholar] - Park, J.; Baek, H. Stereo vision based obstacle collision avoidance for a quadrotor using ellipsoidal bounding box and hierarchical clustering. Aerosp. Sci. Technol.
**2020**, 103, 105882. [Google Scholar] [CrossRef] - Ji, J.; Khajepour, A.; Melek, W.W.; Huang, Y. Path planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints. IEEE Trans. Veh. Technol.
**2017**, 66, 952–964. [Google Scholar] [CrossRef] - Elmokadem, T.; Zribi, M.; Youcef-Toumi, K. Trajectory tracking sliding mode control of underactuated AUVs. Nonlinear Dyn.
**2016**, 84, 1079–1091. [Google Scholar] [CrossRef] [Green Version] - Arrichiello, F.; Chiaverini, S.; Indiveri, G.; Pedone, P. The null-space-based behavioral control for mobile robots with velocity actuator saturations. Int. J. Robot. Res.
**2010**, 29, 1317–1337. [Google Scholar] [CrossRef] - Egeland, O.; Gravdahl, J.T. Modeling and Simulation for Automatic Control; Marine Cybernetics: Trondheim, Norway, 2002; ISBN 9788292356012. [Google Scholar]
- Xiao, S. Multiplication of Quaternion Matrix and Its Feasibility. ACTA Mech. Sin.
**1984**, 16, 159–166. [Google Scholar] - Ellerbroek, J.; Brantegem, K.C.R.; Rene Van Paassen, M.M.; Mulder, M. Design of a coplanar airborne separation display. IEEE Trans. Hum.-Mach. Syst.
**2013**, 43, 277–289. [Google Scholar] [CrossRef] [Green Version] - Oland, E.; Schlanbusch, R.; Kristiansen, R. Underactuated Waypoint Tracking of a Fixed-Wing UAV; IFAC: Compiegne, France, 2013; Volume 2, ISBN 9783902823571. [Google Scholar]
- Guan, J.; Yi, W.J.; Chang, S.J.; Liang, Z.D.; Lyu, Y.P. Study of flight path tracking and control of an UAV in 3D sp7ace. Binggong Xuebao/Acta Armamentarii
**2016**, 37, 64–70. [Google Scholar] - Oland, E.; Kristiansen, R. Trajectory tracking of an underactuated fixed-wing UAV. AIP Conf. Proc.
**2014**, 1637, 1345–1354. [Google Scholar]

**Figure 2.**(

**a**) Geometric elements of the VO cone. (

**b**) The VO cone relative to different coordinate systems.

**Figure 4.**(

**a**) Maximum tracking errors in (m) for the fixed-wing UAV and for varying direction of control velocities ${\mathit{v}}_{avo}$. (

**b**) The flight trajectory of the fixed-wing UAV under varying direction of control velocities. Example in the picture, the initial velocity is set to ${\mathit{v}}_{UAV}$ = 40 m/s and the initial quaternion to ${\mathit{q}}_{bs}={\left[1{\mathbf{0}}^{\mathrm{T}}\right]}^{\mathrm{T}}$.

**Figure 9.**(

**a**) Trajectory of fix-wing UAV and obstacle in inertial coordinate system, (

**b**) distance between fixed-wing UAV and obstacle (Simulation 1).

**Figure 10.**Related parameters of fix-wing UAV (Simulation 1): (

**a**) status signal of UAV, (

**b**) control input, (

**c**) thrust, (

**d**) attitude tracking error, (

**e**) error of angular velocity, (

**f**) velocity in inertial coordinate system, (

**g**) attitude, (

**h**) alpha and beta.

**Figure 11.**(

**a**) Trajectories of fixed-wing UAVs in Inertial coordinate system for several values of δ (Simulation 2), (

**b**) distance between the fixed-wing UAV and obstacle for several values of δ.

Fixed-Wing UAV | 3D-VO Collision Avoidance | Control System | |||
---|---|---|---|---|---|

$m=20.64\text{}\mathrm{kg}$ | ${\mathit{p}}_{UAV}^{n}\left(0\right)={[0\text{}0\text{}-100]}^{\mathrm{{\rm T}}}$ | ${r}_{ps}=100\text{}\mathrm{m}$ | ${d}_{safe}=500\text{}\mathrm{m}$ | ${k}_{1}=2\text{}$ | ${k}_{2}=2$ |

${v}_{UAV}^{b}\left(0\right){=\left[30\text{}0\text{}0\right]}^{\mathrm{{\rm T}}}$ | ${q}_{nb}\left(0\right)={[1\text{}0\text{}0\text{}0]}^{\mathrm{{\rm T}}}$ | ${\lambda}^{\ast}=90\xb0$ | ${v}_{d}=40\text{}\mathrm{m}/\mathrm{s}$ | ${\kappa}_{1}=2$ | ${\kappa}_{2}=2$ |

${w}_{nb}^{b}\left(0\right)={[0\text{}0\text{}0]}^{\mathrm{{\rm T}}}$ | ${\mathit{p}}_{d}^{n}={[20\text{}10\text{}10]}^{\mathrm{{\rm T}}}\ast {10}^{3}$ | ${\alpha}_{\mathrm{max}}=90\xb0$ | $\tau =0.5\mathrm{s}$ | ${K}_{s}=2{\rm I}$ | $\mathrm{\Lambda}=5\text{}\mathrm{m}$ |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Qu, Y.; Yi, W.
Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method. *Appl. Sci.* **2022**, *12*, 955.
https://doi.org/10.3390/app12030955

**AMA Style**

Qu Y, Yi W.
Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method. *Applied Sciences*. 2022; 12(3):955.
https://doi.org/10.3390/app12030955

**Chicago/Turabian Style**

Qu, Yue, and Wenjun Yi.
2022. "Three-Dimensional Obstacle Avoidance Strategy for Fixed-Wing UAVs Based on Quaternion Method" *Applied Sciences* 12, no. 3: 955.
https://doi.org/10.3390/app12030955