Collision Avoidance of Hexacopter UAV Based on LiDAR Data in Dynamic Environment

Abstract

A reactive three-dimensional maneuver strategy for a multirotor Unmanned Aerial Vehicle (UAV) is proposed based on the collision cone approach to avoid potential collision with a single moving obstacle detected by an onboard sensor. A Light Detection And Ranging (LiDAR) system is assumed to be mounted on a hexacopter to obtain the obstacle information from the collected point clouds. The collision cone approach is enhanced to appropriately deal with the moving obstacle with the help of a Kalman filter. The filter estimates the position, velocity, and acceleration of the obstacle by using the LiDAR data as the associated measurement. The obstacle state estimate is utilized to predict the future trajectories of the moving obstacle. The collision detection and obstacle avoidance maneuver decisions are made considering the predicted trajectory of the obstacle. Numerical simulations, including a Monte Carlo campaign, are conducted to verify the performance of the proposed collision avoidance algorithm.

1. Introduction

In the last decade, Unmanned Aerial Vehicles (UAVs) have achieved significant advances in scientific research as well as in real-world applications. Especially, rotary-wing UAVs have been employed for a wide range of application areas due to their vertical takeoff and landing capability. Nowadays, various types of UAVs with different structures keep emerging. For instance, UAVs with six or more rotors have attracted attention due to their higher payload capacity [1,2,3,4,5]. Furthermore, various sensors have been mounted on the UAVs for applications such as autonomous formation flight, surveillance, target tracking, reconnaissance, disaster assistance, etc.

Light Detection And Ranging (LiDAR) is one type of active remote sensing and has been widely employed as a primary ranging device in many autonomous robotic vehicles including UAVs [6,7,8,9,10]. Wallace et al. [11] presented a UAV-borne LiDAR system to collect measurements for forest inventory applications. A Sigma Point Kalman Smoother (SPKS) was applied to combine observations from different sensors. Then, Wallace et al. [12] developed a tree detection algorithm using high-density LiDAR data. The effect of point density on the accuracy of tree detection and delineation was evaluated. On the other hand, Chen et al. [13] integrated LiDAR and UAV to extract detailed surface geologic information. The random sample consensus method was adopted to ascertain the best-fit plane of bedding. Hening et al. [14] presented a data fusion technique for the position and velocity estimation of UAVs. Local position information was provided by a LiDAR sensor, and an adaptive Kalman filter was applied for the integration. Zheng et al. [15] built an obstacle detection system using LiDAR with the consideration of a moving point cloud.

To fully take advantage of UAVs in the real world, a high degree of flight safety related to a close encounter with another UAV should be guaranteed. In a real situation, the obstacle information is unknown prior to deployment and it should be detected by a sensor mounted on the UAV during the mission. The UAV must be capable of avoiding the detected obstacle using the online-collected information. In other words, obstacle detection and avoidance is one of the most important capabilities for the success of UAV applications. Therefore, it is crucial to develop a collision avoidance strategy for the UAV to perform a given mission in a dynamic environment.

Collision avoidance against a moving obstacle has been investigated by various researchers. In the robotics field, Cherubini et al. [16] designed a set of paths during navigation to take into account the obstacle velocity, which was estimated by a Kalman filter. The estimated velocity was used to predict the obstacle’s position within a tentacle-based approach. Ji et al. [17] presented a path planning and tracking framework for autonomous vehicles. The tracking task was formulated as a multiconstrained model predictive control problem, and the front wheel steering angle was calculated to prevent the vehicle from colliding with a moving obstacle. Malone et al. [18] proposed a scheme to incorporate the effect of likely obstacle motion into the desired path of a robot. The method combined an artificial potential field with stochastic reachable set for online path planning. Li et al. [19] developed a controller based on a nonlinear model predictive control for obstacle avoidance. A moving trend function was constructed to predict dynamic obstacle position variances in the prediction horizon. Zhang et al. [20] focused on the tracking of an object in the presence of multiple dynamic obstacles. A method based on conservation of energy was proposed to adjust the motion states of the robot manipulators.

On the other hand, in the aerospace field, Mujumdar and Padhi [21] developed nonlinear geometric guidance and differential geometric guidance, based on aiming point guidance. The guidance law of the UAV was made applicable for moving obstacles by incorporating the Point of Closest Approach (PCA). Wang and Xin [22] addressed the formation control problem where other UAVs are treated as moving obstacles. A nonquadratic avoidance cost function was constructed by an inverse optimal control approach so that the optimal control law was obtained in an analytical form. Yang et al. [23] designed a switching controller to achieve a spatial collision avoidance strategy for fixed-wing UAV. The UAV maintained a safe distance from an intruder by keeping the desired relative bearing and elevation during a collision course. Gageik et al. [24] demonstrated an autonomous flight with low-cost sensors. The movement of an obstacle was handled by the parametrization of the weighted filter. Radmanesh and Kumar [25] formulated a mixed-integer linear program for the path planning problem by incorporating the collision avoidance requirements as the constraints. The constraint relations enforced the formation to visit waypoints and to avoid collision with any intruder aircraft. Lin and Saripalli [26] proposed a sampling-based path planning method based on the Closed-Loop Rapidly-Exploring Random Tree (CL-RRT) algorithm. Using the reachable set, the candidate trajectory method was made robust against unexpected obstacle maneuvers. Santos et al. [27] presented a trajectory tracking controller based on a time-variant artificial potential field with hierarchical objectives using a behavior-based approach. Obstacle motion was taken into account through the variations of the potential function. Marchidan and Bakolas [28] utilized a parameterized vector field that was generated from the decomposition of agent kinematics into normal and tangential components. With a parametric proximity-based eigenvalue function, the agent’s maneuver could be adjusted with little computation effort.

The capability of hovering over a designated position is the unique property of the rotary-wing UAVs enabling various useful applications. For example, with the development of communication techniques, UAV Base Station (UAV-BS) becomes a promising approach to provide wireless connectivity to ground user equipment or other aerial nodes [29,30,31,32]. Moreover, UAV-BS can be utilized in enhancing the coverage and rate performance of the wireless communication networks in scenarios such as an emergency situation. As a base station, it is important for the UAV to perform station keeping at its current location to serve the incoming flow. That is, the performance of the UAV-BS depends on the UAV’s flight time. In this time period, the UAV also estimates the network state and feeds this information to a central agent. Considering its deployment to dangerous sites as a first responder service, the airborne base station should remain intact even in the environment where the presence of an unknown dynamic obstacle is highly likely. Therefore, it is necessary to develop a collision avoidance algorithm which is appropriate during the hovering maneuver of the UAV.

In this study, an enhanced collision avoidance algorithm based on the collision cone approach [33,34] is proposed to deal with a three-dimensional dynamic environment. A hexacopter is considered as the flying platform [35]. The UAV is equipped with a Global Positioning System (GPS) and Attitude and Heading Reference System (AHRS) so that the position and attitude information can be obtained. A conventional Proportional-Derivative (PD) control system is designed to enable waypoint guidance where position and yaw angle commands are fed from the proposed collision avoidance algorithm. An obstacle is completely unknown to the hexacopter initially, and it is detected by a LiDAR system during the flight. From a practical standpoint, the LiDAR has limited Field-of-View (FoV) and sensing range. The obtained obstacle data points are utilized to form a spherical bounding box and collision cone. Also, they are used in an estimator to obtain the state of the obstacle. A discrete-time Kalman filter is adopted with its measurement being the center of the bounding box [36]. The obstacle state estimate is utilized to check the collision condition. The predicted trajectory of the obstacle is considered to detect collision. If a potential collision is detected, the state estimate is used to generate aiming point candidates for the avoidance maneuver. The proposed algorithm removes the candidate points that overlap with the predicted path of the obstacle. This enables the hexacopter to consider the kinematics of the obstacle in choosing the direction of the avoidance maneuver. The point closest to the current velocity of the UAV is chosen as the final aiming point. Numerical simulations are performed to demonstrate the performance of the proposed collision avoidance algorithm. As the previous collision cone approach [37] utilizes the velocity vector of the UAV, its application during the hovering maneuver is difficult, as the velocity is nearly a zero vector. However, the proposed algorithm works well in this situation due to the usage of the relative geometry. That is, the proposed algorithm appropriately handles the avoidance against the moving obstacle, in contrast to the previous algorithm. In addition, a Monte Carlo simulation is performed to verify the robustness of the proposed algorithm.

The remainder of this paper is organized as follows. Section 2 describes the dynamics, control system, and data acquisition system of a hexacopter. The background of the collision cone approach is briefly explained in Section 3. In Section 4, the structure of a discrete-time Kalman filter is presented for the estimation. Then, the collision avoidance algorithm for a moving obstacle is proposed in Section 5. The simulation results are presented in Section 6, and the conclusion is given in Section 7.

2. UAV System

2.1. Dynamics

A hexacopter is configured with six rotors, where three rotors rotate in clockwise direction and the others rotate in counterclockwise direction. A platform of the Tarot 680 hexacopter is shown in Figure 1 as an example. A North-East-Down (NED) coordinate system is considered in an inertial frame. Note that flat-Earth and rigid body are assumed in this study. A body-fixed coordinate system is defined with its origin at the hexacopter’s center of mass and its axes ($x^b$, $y^b$, and $z^b$) are aligned with the reference direction vector, as shown in Figure 1.

The rigid hexacopter dynamics is governed by the following translational and rotational differential equations [35].

$${\dot{\overrightarrow{r}}}^n=C_{b/n}^T{\overrightarrow{v}}^b$$

(1)

$${\dot{\overrightarrow{v}}}^b=\frac{1}{m}{\overrightarrow{F}}^b+C_{b/n}{\overrightarrow{g}}^n-{\overrightarrow{\omega }}^b\times {\overrightarrow{v}}^b$$

(2)

$${\dot{\overrightarrow{\omega }}}^b=J^{-1}\left({\overrightarrow{M}}^b-{\overrightarrow{\omega }}^b\times J{\overrightarrow{\omega }}^b\right)$$

(3)

where ${\overrightarrow{r}}^n\equiv {\left[x^n{y}^n{z}^n\right]}^T$ is the position vector of the hexacopter represented in the NED coordinate system; ${\overrightarrow{v}}^b\equiv {\left[uvw\right]}^T$ and ${\overrightarrow{\omega }}^b\equiv {\left[pqr\right]}^T$ are the velocity vector and the angular velocity of the hexacopter represented in the body-fixed coordinate system, respectively; m and $J\equiv diag[J_x,J_y,J_z]$ are the mass and inertia matrix of the hexacopter, respectively; ${\overrightarrow{F}}^b$ is the external force of the hexacopter represented in the body-fixed coordinate system; ${\overrightarrow{M}}^b$ is the external moment of the hexacopter represented in the body-fixed coordinate system; ${\overrightarrow{g}}^n\equiv {\left[00g\right]}^T$ is the gravity vector represented in the NED coordinate system; and $C_{b/n}$ ($=C_{n/b}^T$) is the direction cosine matrix performing coordinate transformation from the NED coordinate system to the body-fixed coordinate system, which is defined with a (3-2-1) Euler sequence as follows,

$$C_{b/n}=\left[\begin{array}{ccc}cos\theta cos\psi & cos\theta sin\psi & -sin\theta \\ -cos\varphi sin\psi +sin\varphi sin\theta cos\psi & cos\varphi cos\psi +sin\varphi sin\theta sin\psi & sin\varphi cos\theta \\ sin\varphi sin\psi +cos\varphi sin\theta cos\psi & -sin\varphi cos\psi +cos\varphi sin\theta sin\psi & cos\varphi cos\theta \end{array}\right]$$

(4)

where Euler angle $\overrightarrow{\Phi }\equiv {\left[\varphi \theta \psi \right]}^T$ is defined between the NED and body-fixed coordinate systems.

The external force and moment of the hexacopter can be expressed as follows.

$${\overrightarrow{F}}^b=\left[\begin{array}{c}0\\ 0\\ -T\end{array}\right],{\overrightarrow{M}}^b=\left[\begin{array}{c}L\\ M\\ N\end{array}\right]$$

(5)

Then, ${\overrightarrow{F}}^b$ and ${\overrightarrow{M}}^b$ have the following relationship with the angular velocity of the rotor,

$$\left[\begin{array}{c}T\\ L\\ M\\ N\end{array}\right]=\left[\begin{array}{cccccc}{k}_f& k_f& k_f& k_f& k_f& k_f\\ -\frac{1}{2}{k}_{f}l& -k_{f}l& -\frac{1}{2}{k}_{f}l& \frac{1}{2}{k}_{f}l& k_{f}l& \frac{1}{2}{k}_{f}l\\ \frac{\sqrt{3}}{2}{k}_{f}l& 0& -\frac{\sqrt{3}}{2}{k}_{f}l& -\frac{\sqrt{3}}{2}{k}_{f}l& 0& \frac{\sqrt{3}}{2}{k}_{f}l\\ k_{\tau }& -k_{\tau }& k_{\tau }& -k_{\tau }& k_{\tau }& -k_{\tau }\end{array}\right]\left[\begin{array}{c}{w}_1^2\\ w_2^2\\ w_3^2\\ w_4^2\\ w_5^2\\ w_6^2\end{array}\right]$$

(6)

where $k_f$ and $k_{\tau }$ are the thrust and drag coefficients of the rotor, respectively; l is the distance from the hexacopter’s center of mass to the rotor; and $w_i$ is the angular velocity of the i-th rotor with $i\in \{1,\cdots ,6\}$.

2.2. Control Design

In this study, a PD controller is designed to track the desired position vector ($x_d^n$, $y_d^n$, and $z_d^n$) and yaw angle (${\psi }_d$). First, the collective thrust command, T, is computed to handle the desired altitude of the hexacopter, which is represented by $z_d^n$.

$$T=m\frac{2{\zeta }_v{\omega }_v{\dot{z}}^n+{\omega }_v^2(z^n-z_d^n)+g}{cos\varphi cos\theta }$$

(7)

where ${\zeta }_v$ and ${\omega }_v$ are design parameters. The horizontal position controller equations are given as

$$a_{x,cmd}=-2{\zeta }_h{\omega }_h{\dot{x}}^n+{\omega }_h^2(x_d^n-x^n)$$

(8)

$$a_{y,cmd}=-2{\zeta }_h{\omega }_h{\dot{y}}^n+{\omega }_h^2(y_d^n-y^n)$$

(9)

$$\left[\begin{array}{c}{a}_{u,cmd}\\ a_{v,cmd}\\ a_{w,cmd}\end{array}\right]=C_{b/n}\left[\begin{array}{c}{a}_{x,cmd}\\ a_{y,cmd}\\ 0\end{array}\right]$$

(10)

where ${\zeta }_h$ and ${\omega }_h$ are design parameters. Now, the desired roll and pitch angles, ${\varphi }_d$ and ${\theta }_d$, respectively, can be obtained as follows.

$${\varphi }_d=\mathrm{atan}2(a_{v,cmd},g)$$

(11)

$${\theta }_d=\mathrm{atan}2(-a_{u,cmd},g)$$

(12)

Then, the commanded roll, pitch, and yaw moments, L, M, and N, respectively, can be computed for the attitude loop as follows,

$$L=J_x\left(-2{\zeta }_{\varphi }{\omega }_{\varphi }p+{\omega }_{\varphi }^2({\varphi }_d-\varphi )\right)$$

(13)

$$M=J_y\left(-2{\zeta }_{\theta }{\omega }_{\theta }q+{\omega }_{\theta }^2({\theta }_d-\theta )\right)$$

(14)

$$N=J_z\left(-2{\zeta }_{\psi }{\omega }_{\psi }p+{\omega }_{\psi }^2({\psi }_d-\psi )\right)$$

(15)

where ${\zeta }_{\varphi }$, ${\zeta }_{\theta }$, ${\zeta }_{\psi }$, ${\omega }_{\varphi }$, ${\omega }_{\theta }$, and ${\omega }_{\psi }$ are design parameters. Finally, the angular velocities of the six rotors are allocated by using Equation (6) with T, L, M, and N obtained in Equations (7), (13)–(15), respectively. Note that $x_d^n$, $y_d^n$, $z_d^n$, and ${\psi }_d$ are delivered from the proposed collision avoidance guidance system described in Section 5.

2.3. LiDAR Data Sensing

Let us assume that an obstacle consists of a point cloud that represents its external surface. The LiDAR with limited FoV is mounted on the hexacopter and directed along the $+x^b$-axis to acquire obstacle data. The visibility of the obstacle point by the LiDAR can be determined by examining whether the obstacle data point is inside a rectangular pyramid whose vertices are $o^b$, $v_1$, $v_2$, $v_3$, and $v_4$, as shown in Figure 2. $v_1$, $v_2$, $v_3$, and $v_4$ in the body-fixed coordinate system can be obtained as follows.

$$\begin{array}{ccc}\hfill v_1& =& {\left[\begin{array}{ccc}{d}_a& -d_{a}tan(H_{fv}/2)& -d_{a}tan(V_{fv}/2)\end{array}\right]}^T\hfill \\ \hfill v_2& =& {\left[\begin{array}{ccc}{d}_a& d_{a}tan(H_{fv}/2)& -d_{a}tan(V_{fv}/2)\end{array}\right]}^T\hfill \\ \hfill v_3& =& {\left[\begin{array}{ccc}{d}_a& d_{a}tan(H_{fv}/2)& d_{a}tan(V_{fv}/2)\end{array}\right]}^T\hfill \\ \hfill v_4& =& {\left[\begin{array}{ccc}{d}_a& -d_{a}tan(H_{fv}/2)& d_{a}tan(V_{fv}/2)\end{array}\right]}^T\hfill \end{array}$$

(16)

where $H_{fv}$ and $V_{fv}$ are the horizontal and vertical FoV of the LiDAR, respectively, and $d_a$ is a sensing range of the LiDAR. The hexacopter performs its mission with constant obstacle data acquisition from the LiDAR.

3. Review of Basic Collision Cone Approach

In this study, the collision cone method is adopted as the basis of the collision detection and avoidance logic [33,34]. Let us assume that $n_c$ data points are accumulated by the LiDAR, and that the position vector of the k-th obstacle data point in the NED coordinate system is defined as ${\overrightarrow{r}}_{o,k}^n$. Among $n_c$ data points, there are two points associated with the longest pairwise Euclidean distance. Let us define these points as ${\overrightarrow{r}}_{o,1}^n$ and ${\overrightarrow{r}}_{o,2}^n$. Then, the center of a spherical bounding box can be obtained as follows.

$${\overrightarrow{r}}_{o,c}^n=\frac{{\overrightarrow{r}}_{o,1}^n+{\overrightarrow{r}}_{o,2}^n}{2}$$

(17)

Also, the radius of the bounding box can be designed as follows,

$$r_s=\frac{\Vert {\overrightarrow{r}}_{o,1}^n-{\overrightarrow{r}}_{o,2}^n{\Vert }_2}{2}+d_{sm}$$

(18)

where $d_{sm}$ is a safety margin. Finally, a collision cone can be constructed with a set of tangent lines from the hexacopter to the bounding box, as shown in Figure 3.

If the velocity vector of the hexacopter is contained within the collision cone, the detected obstacle is a potential threat to the UAV unless it changes its trajectory. In this case, the hexacopter should perform a collision avoidance maneuver. During the collision avoidance maneuver, the hexacopter is guided to an aiming point instead of the predefined goal point. The aiming point is selected from the points on the intersection curve of the bounding box and the collision cone. In this study, the point closest to the velocity vector of the hexacopter is chosen as the aiming point to minimize the UAV’s maneuver. If the velocity vector of the hexacopter is outside the collision cone, then the UAV is guided to the original goal point. This logic is repeated until the hexacopter reaches the goal point.

4. Obstacle State Estimator

As a moving obstacle is considered in this study, its state should be estimated to implement the proposed collision avoidance algorithm described in Section 5. A discrete-time Kalman filter is constructed for the estimation [36]. First, the filter state is defined as follows,

$${\widehat{\overrightarrow{x}}}_k^n\equiv {\left[\begin{array}{ccccccccc}{x}_{e,k}^n& {\dot{x}}_{e,k}^n& {\ddot{x}}_{e,k}^n& y_{e,k}^n& {\dot{y}}_{e,k}^n& {\ddot{y}}_{e,k}^n& z_{e,k}^n& {\dot{z}}_{e,k}^n& {\ddot{z}}_{e,k}^n\end{array}\right]}^T$$

(19)

where ${\widehat{\overrightarrow{r}}}_{e,k}^n\equiv {\left[x_{e,k}^n{y}_{e,k}^n{z}_{e,k}^n\right]}^T$, ${\widehat{\overrightarrow{v}}}_{e,k}^n\equiv {\left[{\dot{x}}_{e,k}^n{\dot{y}}_{e,k}^n{\dot{z}}_{e,k}^n\right]}^T$, and ${\widehat{\overrightarrow{a}}}_{e,k}^n\equiv {\left[{\ddot{x}}_{e,k}^n{\ddot{y}}_{e,k}^n{\ddot{z}}_{e,k}^n\right]}^T$ are the position, velocity, and acceleration estimates of the obstacle, respectively, represented in the NED coordinate system. Now, the discrete-time state equation for the estimator can be expressed as follows,

$${\widehat{\overrightarrow{x}}}_{k+1}^n=\left[\begin{array}{ccccccccc}1& \Delta t_l& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& \Delta t_l& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& \Delta t_l& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& \Delta t_l& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& \Delta t_l& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& \Delta t_l\\ 0& 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]{\widehat{\overrightarrow{x}}}_k^n+{\overrightarrow{w}}_e$$

(20)

where $\Delta t_l$ is the sampling time of LiDAR data acquisition, and ${\overrightarrow{w}}_e\equiv {\left[w_x{w}_{\dot{x}}{w}_{\ddot{x}}{w}_y{w}_{\dot{y}}{w}_{\ddot{y}}{w}_z{w}_{\dot{z}}{w}_{\ddot{z}}\right]}^T$ is a process noise characterized by $N(0,Q_e)$. On the other hand, the position vector of the center of the bounding box obtained in Equation (17) by the LiDAR system is used as a measurement. Therefore, the measurement equation can be constructed as follows,

$${\overrightarrow{y}}_k^n=\left[\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0\end{array}\right]{\widehat{\overrightarrow{x}}}_k^n+{\overrightarrow{v}}_e$$

(21)

where ${\overrightarrow{v}}_e\sim N(0,R_e)$ is a measurement noise. In this study, an unknown obstacle is detected by LiDAR sensor that has a limited sensing range. The bounding box enclosing the set of detected data points gradually grows in its size as more of the obstacle comes within the sensing range. In this case, ${\overrightarrow{r}}_{o,c}^n$, computed in Equation (17), may differ from the actual center of the obstacle. This error can be treated as a measurement noise, ${\overrightarrow{v}}_e$, in the observation model. Finally, using Equations (20) and (21), the discrete-time Kalman filter can be implemented to estimate the state of the obstacle.

The Kalman filter is initiated when the obstacle is detected by the LiDAR for the first time. Then, it is updated at every $\Delta t_l$ with newly measured ${\overrightarrow{r}}_{o,c}^n$. If ${\overrightarrow{r}}_{o,c}^n$ cannot be found at a certain step, then virtual measurement, ${\widehat{\overrightarrow{y}}}_k^n$, is modeled to keep the estimator running. This situation may occur when the UAV completely avoids the obstacle and every obstacle data point is out of the LiDAR’s sensing range. The virtual measurement, ${\widehat{\overrightarrow{y}}}_k^n$, is modeled with the state estimate at the previous step as follows.

$${\widehat{\overrightarrow{y}}}_k^n=\frac{1}{2}\Delta t_l^2\left[\begin{array}{c}{\ddot{x}}_{e,k-1}^n\\ {\ddot{y}}_{e,k-1}^n\\ {\ddot{z}}_{e,k-1}^n\end{array}\right]+\Delta t_l\left[\begin{array}{c}{\dot{x}}_{e,k-1}^n\\ {\dot{y}}_{e,k-1}^n\\ {\dot{z}}_{e,k-1}^n\end{array}\right]+\left[\begin{array}{c}{x}_{e,k-1}^n\\ y_{e,k-1}^n\\ z_{e,k-1}^n\end{array}\right]$$

(22)

When the obstacle is detected again, ${\widehat{\overrightarrow{y}}}_k^n$ is discarded and the Kalman filter operates with the normal measurement.

5. Collision Avoidance Strategy Against Moving Obstacle

As the Kalman filter provides the obstacle information, these state estimates are utilized in the collision detection algorithm and avoidance law considering the dynamic environment. The proposed strategy proceeds in several stages as shown in Figure 4.

Although an obstacle is within the range of the LiDAR, the velocity vector of the hexacopter is outside the collision cone at $t=t_0$, as shown in Figure 4a. Then, the previous approach determines that the UAV is not on a collision course to the obstacle and allows the UAV to continue flying toward the goal point, as it only incorporates the current position and velocity in collision detection. On the contrary, the method proposed in this study utilizes the instantaneous short-term predictions for the obstacle trajectory to determine whether the UAV is on the course to close proximity of the obstacle. The trajectory of the obstacle is predicted by utilizing the state estimate as

$${\widehat{\overrightarrow{r}}}_e^n=\frac{1}{2}{\widehat{\overrightarrow{a}}}_{e,k}^n\Delta t_d^2+{\widehat{\overrightarrow{v}}}_{e,k}^n\Delta t_d+{\widehat{\overrightarrow{r}}}_{e,k}^n$$

(23)

with the prediction horizon, $\Delta t_d$, of various lengths. Also, the future position of the hexacopter can be predicted as follows,

$${\overrightarrow{r}}_e^n={\overrightarrow{v}}^n\Delta t_d+{\overrightarrow{r}}^n$$

(24)

where ${\overrightarrow{v}}^n$ is the velocity vector of the hexacopter represented in the NED coordinate system. Note that the acceleration vector is not used in Equation (24) because the abrupt change in the acceleration by the proposed algorithm may result in poor prediction. If the Euclidean distance between the predicted obstacle position, ${\widehat{\overrightarrow{r}}}_e^n$, and the predicted UAV position, ${\overrightarrow{r}}_e^n$, is smaller than the bounding sphere radius, $r_s$, at a certain $\Delta t_d$, the UAV determines that it is on a collision course. Let us define this moment as $t=t_a$, as shown in Figure 4b. Additionally, note that $r_s$ is constantly calculated using Equation (18) whenever the obstacle is detected by the LiDAR and the algorithm keeps updating the maximum observed $r_s$ value for safety reasons, yet this approach might be conservative. The hexacopter is inside the spherical bounding box of the obstacle in Figure 4b, which means a collision.

Now, a collision avoidance maneuver is required. To perform the avoidance maneuver, the collision cone is constructed with the bounding box at $t=t_a$ and the position of the UAV at $t=t_0$, as shown in Figure 4c. The aiming point for the UAV chosen in the direction of the obstacle’s future trajectory is likely to increase the chance of a close encounter with the obstacle at the next time step, which is disadvantageous from the viewpoint of minimizing the probability of a possible collision. An additional pruning process is therefore conducted after creating the aiming point candidates to prevent undesirable and ineffective collision avoidance maneuver due to a wrong choice of aiming point. The trace of bounding boxes starting from $t=t_a$ can be created as shown in Figure 4c with the prediction horizon given by $\Delta t_p$. These boxes can then be used to trim the candidate points. If an aiming point candidate is inside the predicted envelope of the bounding boxes at any moment, then that point is eliminated from the candidate set.

Some candidate points may be removed by the pruning process as shown in Figure 4d. Then, the final aiming point is chosen from the remaining candidate points. Even though the point closest to the velocity vector of the hexacopter is chosen, the chosen point may be far from the velocity vector because of the elimination step, as shown in Figure 4d.

The aiming point provided as the final result of the collision avoidance strategy comprises the desired position part of the guidance command. Meanwhile, the desired yaw angle is designed so that the LiDAR faces toward the center of the spherical bounding box.

$${\psi }_d=arctan\frac{y_{e,k}^n-y^n}{x_{e,k}^n-x^n}$$

(25)

The reasoning behind the yaw angle command design is to keep the obstacle inside of the LiDAR’s FoV for the consecutive state estimation.

The overall flowchart of the proposed collision avoidance algorithm is shown in Figure 5.

6. Numerical Simulation

Numerical simulations are conducted to demonstrate the performance of the proposed collision avoidance algorithm. The parameters of the hexacopter dynamics and control system, estimator, and collision avoidance used in the numerical simulation are listed in

Table 1. Parameters of the hexacopter, Kalman filter, and collision avoidance used in numerical simulation.

Hexacopter dynamics
m	2.356 kg	$J_x$	$1.676\times {10}^{-1}$ kg m2	$J_y$	$1.676\times {10}^{-1}$ kg m2	$J_z$	$2.974\times {10}^{-1}$ kg m2
l	0.5 m	$k_f$	$1.1\times {10}^{-1}$ Ns2	$k_{\tau }$	$5.2\times {10}^{-2}$ Ns2 m	g	9.8 m/s2
Hexacopter controller
${\omega }_v$	3 rad/s2	${\zeta }_v$	0.8	${\omega }_h$	3 rad/s2	${\zeta }_h$	0.8
${\omega }_{\varphi }$	15 rad/s2	${\zeta }_{\varphi }$	0.7	${\omega }_{\theta }$	15 rad/s2	${\zeta }_{\theta }$	0.7
${\omega }_{\psi }$	5 rad/s2	${\zeta }_{\psi }$	0.9
LiDAR sensor
$\Delta t_l$	0.1 s	$d_a$	10 m	$H_{fv}$	170°	$V_{fv}$	30°
Kalman filter
${\widehat{\overrightarrow{x}}}_0^n$	${\left[000000000\right]}^T$			$P_0^{+}$	diag$[0,0,0,0,0,0,0,0,0]$
$Q_e$	diag$[1\times {10}^{-8},1\times {10}^{-4},1,1\times {10}^{-8},1\times {10}^{-4},1,1\times {10}^{-8},1\times {10}^{-4},1]$
$R_e$	diag$[1\times {10}^{-8},1\times {10}^{-8},1\times {10}^{-8}]$
Collision avoidance
$d_{sm}$	2 m	Max. $\Delta t_d$	30 s	$\Delta t_p$	0.1 s

.

The time step for numerical integration and control update is 0.01 s, whereas the sampling time of the LiDAR data acquisition, $\Delta t_l$, is set to 0.1 s. The slower rate for the LiDAR measurement update is due to the large amount of computational effort required to process the point cloud data in each cycle. Not only the obstacle state estimator, but also the collision avoidance guidance algorithm, is triggered by the arrival of new LiDAR data. Consequently, they have the same update rate of 10Hz, and the UAV’s autopilot utilizes the latest updated command.

For the control system of the hexacopter, ${\varphi }_d$ and ${\theta }_d$ in Equations (11) and (12) are limited in the range of ±20° to avoid excessive maneuver. Meanwhile, $P_0^{+}$ in Table 1 is the covariance of the initial estimation error of ${\widehat{\overrightarrow{x}}}_0^n$, where diag refers to a square matrix with the elements inside the square bracket being the diagonal entries. As an obstacle is completely unknown initially, there is no choice but to set ${\widehat{\overrightarrow{x}}}_0^n$ as a zero vector. Additionally, the Kalman filter takes time for the state estimate to converge to a steady-state value. Therefore, the proposed collision avoidance algorithm begins to incorporate the obstacle state estimate a few time steps after the first detection of the obstacle in order to allow a period for the filter state to converge. In this study, $5\Delta t_l$ (5 steps) is chosen as this period.

For every simulation in this section, the initial position of the hexacopter is set to ${[00-3]}^T$ in the NED coordinate system. Also, the initial velocity, angular velocity, and Euler angle of the hexacopter are set to zero.

6.1. Simulation I: Hovering Situation

In Simulation I, the hovering situation is considered, which is the case where the goal point is given by the initial position of the UAV. Namely, the hexacopter stays at ${[00-3]}^T$ if a detected obstacle is not considered as a potential threat. An obstacle whose shape is a sphere with the radius of 1 m is considered. Its initial position is set to ${[180-3]}^T$ with a constant velocity of ${[-300]}^T$ so that it is on a collision course.

Figure 6 shows the trajectories of the hexacopter and the obstacle in the NED coordinate system.

Additionally, Figure 7 shows the time responses of the attitude, angular velocity, velocity, and absolute velocity of the hexacopter, and the minimum distance between the UAV and the obstacle. Also, Figure 8 shows the time responses of the estimated state along with the true state plotted as a reference of comparison. Note that all state estimates are set to zero at initial state.

Initially ($t=0$ s), the obstacle is outside the sensing range of the LiDAR, because $d_a$ is set to 10 m in Table 1. Then, the obstacle is first detected by the hexacopter at point A ($t=2.4$ s), as shown in Figure 7c. However, the UAV does not immediately perform avoidance maneuver, but simply continues hovering. This is because the collision avoidance algorithm incorporates the estimated obstacle information only after the filter transients are died out, as explained above. Moreover, only a small part of the obstacle enters the range of the LiDAR at point A. The computed ${\overrightarrow{r}}_{o,c}^n$ at point A is ${[9.980-3]}^T$, whereas its true state is ${[10.80-3]}^T$. This difference may result in a poor estimation. As shown in Figure 8, the state estimates are still at initial zero values at point A. However, after $0.5$ s, which is denoted as point B, most of the state estimates are converged to the true states and these are used in the collision avoidance algorithm. Now, the hexacopter predicts the trajectory of the obstacle, and determines that it is on a collision course.

At point B ($t=2.9$ s), the hexacopter triggers the collision avoidance maneuver by guiding to the aiming point, instead of the goal point. At the same time, the yaw angle command is generated to direct toward ${\left[x_{e,k}^n{y}_{e,k}^n{z}_{e,k}^n\right]}^T$ so that the detected obstacle remains inside the LiDAR’s FoV, as shown in Figure 7a. At point C ($t=4.4$ s), the UAV determines that the future position of the obstacle does not collide with it. That is, the hexacopter is outside of the predicted bounding boxes of the obstacle. Although the UAV heads back to the goal point from this moment, it produces the different trajectory compared to the past one as shown in Figure 6. This is due to the control system of the hexacopter, which generates the change of $\theta$ in advance of $\varphi$, as shown in Figure 7a. Simultaneously, $+x^b$-axis of the hexacopter is aligned with the goal point by adjusting $\psi$.

At point D ($t=6.98$ s), the hexacopter reaches the minimum distance of $2.9353$ m as shown in Figure 7c. However, considering the direction of the obstacle’s velocity vector, it is no longer a threat to the UAV. Finally, at point E ($t=8.7$ s), the hexacopter reaches the goal point as shown in Figure 6 and is able to perform hovering maneuver again.

In the hovering situation, the velocity vector of the hexacopter is a zero vector, which means that it is difficult to utilize the previous collision cone approach [37]. On the contrary, the proposed algorithm gives consideration to the relative motion as it uses the obstacle information estimated by the Kalman filter. Therefore, the proposed algorithm can be adopted while the UAV is in the hovering state.

6.2. Simulation II: Waypoint Guidance

In Simulation II, the proposed collision avoidance algorithm is compared to the previous algorithm [37]. The goal point is set to ${[250-3]}^T$, and the hexacopter initially heads for the goal point. A sphere with a radius of $2.2$ m is initially located at ${[21-10-3]}^T$ and flies toward the path of the UAV. The magnitudes of the velocity vector and the acceleration vector are $2.4$ m/s and $0.25$ m/s², respectively. Also, their unit vector is ${[-0.560.830.03]}^T$.

Figure 9 shows the trajectories of the hexacopter with the proposed algorithm and the previous algorithm, and the obstacle in the NED coordinate system. Also, Figure 10 shows the time responses of the attitude, angular velocity, velocity, and absolute velocity of the hexacopter, and the minimum distance between the UAV and the obstacle. Figure 11 shows the time responses of the state estimate of the Kalman filter.

The obstacle enters the range and FoV of the LiDAR at point A (= point A’, $t=2.4$ s) for both the proposed and previous algorithms, as shown in Figure 9 and Figure 10c. Then, the hexacopter with the proposed algorithm starts collision avoidance maneuver at point B ($t=2.9$ s), as the UAV collides with one of the predicted bounding boxes. As shown in Figure 11, the state estimates show sufficient convergence to the true states from point B, which are appropriate to be utilized in the collision avoidance algorithm. On the other hand, the hexacopter with the previous algorithm triggers the avoidance maneuver at point B’ ($t=2.6$ s), which is earlier than the one with the proposed algorithm. This is because the previous algorithm does not utilize the obstacle state estimator. As soon as the computed collision cone includes the velocity vector of the UAV, the UAV heads for the aiming point as shown in Figure 9.

The difference can be witnessed from this moment. As a benefit of incorporating the pruning step in the aiming point processing pipeline, the proposed algorithm selects the aiming point on the opposite side of the obstacle’s predicted trajectory. However, the previous algorithm chooses the aiming point without considering the short-term future movement of the obstacle. As a consequence, the UAV cannot withstand the acceleration and the velocity of the obstacle and eventually collides with it at point C’ ($t=4.26$ s), as shown in Figure 9. Note that the minimum distance between the UAV and the obstacle is 0 m at point C’ in Figure 10c. That is, although the previous algorithm shows earlier transition to the collision avoidance mode than the proposed algorithm, it fails to avoid the moving obstacle.

On the contrary, the UAV with the proposed algorithm successfully avoids the obstacle and reaches the minimum distance of $2.0733$ m at point C ($t=4.01$ s) as shown in Figure 10c. This is a reasonable value, considering that $d_{sm}$ is set to 2 m in Table 1. At point D ($t=4.1$ s), the hexacopter determines that the obstacle is no longer a threat, and switches to the goal point guidance. Note that there is a time lag between point C and point D due to the sampling time of the LiDAR, $\Delta t_l$.

At point E ($t=6.6$ s), the obstacle deviates from the sensing range of the LiDAR, as shown in Figure 10c, and ${\overrightarrow{r}}_{o,c}^n$ cannot be obtained from this point. Then, the virtual measurement, ${\widehat{\overrightarrow{y}}}_k^n$, is calculated using Equation (22) to produce the measurement of Equation (21) for the Kalman filter. However, the deviation from the true state can be observed before point E, especially in Figure 11b,e. This is because of the incorrect calculation of ${\overrightarrow{r}}_{o,c}^n$ with partial detection of the obstacle. Nevertheless, the goal point guidance is maintained. Finally, the UAV reaches the point F ($t=9.08$ s) as shown in Figure 9.

6.3. Simulation III: Monte Carlo Simulation

A Monte Carlo simulation is carried out to investigate the robustness of the proposed algorithm. Five-hundred different scenarios are generated, where the following properties of the obstacle are assigned using a continuous uniform distribution: impact point, initial position, radius, velocity, and acceleration. First, the impact point with the UAV is randomly chosen between the initial position of the hexacopter, ${[00-3]}^T$, and the goal point, ${[250-3]}^T$, as shown in Figure 12.

Let us define this impact point as ${\overrightarrow{r}}_p^n\equiv {[{\ell }_{p}0-3]}^T$ where ${\ell }_p\in [0,25]$ is shown in Figure 12. Then, the time-to-go of the UAV to the point can be calculated as follows,

$$t_{go}=\frac{{\ell }_p}{v_{avg}}$$

(26)

where $v_{avg}$ is the average speed of the hexacopter. Referring to Figure 7d and Figure 10d, $v_{avg}$ is roughly chosen as $3.5$ m/s. Note that the generated cases with $t_{go}\le 2.0$ s are excluded, considering a reasonable initial gap between the UAV and the obstacle. Then, the ranges of the magnitude of the acceleration and the velocity are set to $[0,2]$ and $[2,6]$, respectively. If these are denoted as $a_o$ and $v_o$, the length between ${\overrightarrow{r}}_p^n$ and the initial position of the obstacle can be computed as follows.

$$L_p=\frac{1}{2}{a}_o{t}_{go}^2+v_o{t}_{go}$$

(27)

Now, ${\theta }_p$ and ${\phi }_p$ in Figure 12 are randomly chosen from $[0,\pi ]$ and $[0,2\pi ]$, respectively, which determine the impact angle. The initial position of the obstacle, ${\overrightarrow{r}}_o^n$, can be obtained as follows.

$${\overrightarrow{r}}_o^n=\left[\begin{array}{c}{L}_{p}sin{\theta }_{p}cos{\phi }_p\\ L_{p}sin{\theta }_{p}sin{\phi }_p\\ L_{p}cos{\theta }_p\end{array}\right]+{\overrightarrow{r}}_p^n$$

(28)

Also, the acceleration, ${\overrightarrow{a}}_o^n$, and the velocity, ${\overrightarrow{v}}_o^n$, of the obstacle can be computed as follows.

$${\overrightarrow{a}}_o^n=\left[\begin{array}{c}-a_{o}sin{\theta }_{p}cos{\phi }_p\\ -a_{o}sin{\theta }_{p}sin{\phi }_p\\ -a_{o}cos{\theta }_p\end{array}\right],{\overrightarrow{v}}_o^n=\left[\begin{array}{c}-v_{o}sin{\theta }_{p}cos{\phi }_p\\ -v_{o}sin{\theta }_{p}sin{\phi }_p\\ -v_{o}cos{\theta }_p\end{array}\right]$$

(29)

Finally, the radius of the obstacle is chosen in the range of $[1,3]$. Note that the cases with the obstacle approaching from the direction outside the FoV of the LiDAR are also excluded from the result.

The Monte Carlo simulation result is shown in Figure 13, which is the time responses of the minimum distance between the hexacopter and the obstacle for each case. In every case, the hexacopter reaches the goal point without violating the safety requirement, as the minimum separation is greater than the added safety margin, $d_{sm}$, at all times. The Monte Carlo simulation result confirms the successful collision avoidance performance of the proposed algorithm.

7. Conclusions

In this study, a collision avoidance algorithm of a hexacopter UAV is proposed. The algorithm is based on the collision cone approach, and is improved to handle a moving obstacle by considering the short-term predictions for the obstacle trajectory with the aid of an obstacle state estimator. A PD controller is used to generate angular velocities of the rotors where the command is fed from the proposed guidance algorithm. A LiDAR system on the UAV obtains the obstacle data points to form the spherical bounding box around the detected region of the obstacle and also the collision cone. Sometimes, the LiDAR system may lose the obstacle due to its limited sensing range and FoV. In this case, the Kalman filter produces a virtual measurement with the previous state estimate. A collision is detected by examining the predicted position of both the UAV and the obstacle. The obstacle state estimate also contributes to an avoidance maneuver of UAV by removing the points that are unreasonable to be taken as candidates of the aiming point. The numerical simulation results, including a Monte Carlo campaign, are shown to verify the proposed guidance algorithm. The results demonstrate that the hexacopter properly handles the moving obstacle; unlike the previous method that does not explicitly consider the predicted relative movement. For future research, the proposed algorithm will be extended to deal with multiple obstacles. An appropriate decision-making algorithm should be developed to assess the obstacles in terms of the level of threat.