# Identification and Avoidance of Static and Dynamic Obstacles on Point Cloud
for UAVs Navigation

Han Chen1 and Peng Lu1,2∗ 1The Adaptive Robotic Controls Lab (ArcLab), Hong
Kong Polytechnic University, Hung Hom, Kowloon, Hong Kong, China.
<EMAIL_ADDRESS>of Mechanical Engineering, The
University of Hong Kong, Pokfulam, Hong Kong, China.
<EMAIL_ADDRESS>author

###### Abstract

Avoiding hybrid obstacles in unknown scenarios with an efficient flight
strategy is a key challenge for unmanned aerial vehicle applications. In this
paper, we introduce a technique to distinguish dynamic obstacles from static
ones with only point cloud input. Then, a computationally efficient obstacle
avoidance motion planning approach is proposed and it is in line with an
improved relative velocity method. The approach is able to avoid both static
obstacles and dynamic ones in the same framework. For static and dynamic
obstacles, the collision check and motion constraints are different, and they
are integrated into one framework efficiently. In addition, we present several
techniques to improve the algorithm performance and deal with the time gap
between different submodules. The proposed approach is implemented to run
onboard in real-time and validated extensively in simulation and hardware
tests. Our average single step calculating time is less than 20 ms.

##  I INTRODUCTION

In unknown and chaotic environments, unmanned aerial vehicles (UAVs),
especially quadcopters always face rapid unexpected changes, while moving
obstacles pose a greater threat than static ones. To tackle this challenge,
the trajectory planner for UAVs needs to constantly and quickly generate
collision-free and feasible trajectories in different scenarios, and its
response time needs to be as short as possible. In addition, the optimality of
the motion strategies should also be considered to save the limited energy of
quadrotors.

Most existing frameworks that enable drones to generate collision-free
trajectories in completely unknown environments only take into consideration
stationary obstacles. However, as quadrotors often fly at low altitudes, they
are faced with many moving obstacles such as vehicles and pedestrians on the
ground. One primary solution to avoid collision is to raise flight altitude to
fly above all the obstacles. This method is not feasible for some indoor
applications, because the flight altitude is lower in narrow indoor space, and
the drones are often requested to interact with humans as well. Another
solution is to assume all detected obstacles as static. But this method cannot
guarantee the safety of the trajectory [1], considering measurement errors
from the sensors and unmissable calculating time of the planner. Therefore, a
more efficient and safer way to avoid moving obstacles is to predict and
consider the obstacles’ position in advance based on the velocity, which can
avoid detours or deadlock on some occasions.

Figure 1: The composite picture of the simulation in Gazebo for the process
that the drone avoids static and dynamic obstacles. 5 screenshots are used for
composition and the cut time interval is fixed to 0.7 seconds. The line with
an arrowhead shows the moving direction and the numbers mark the corresponding
frame, the numbers increase by time. The yellow line is generated by the
method in this paper, while the red line is by the original static method. The
corresponding visualized data is shown in Fig. 8(c).

In this paper, we propose a framework to distinguish and avoid the dynamic and
static obstacles, including a raw point cloud filter, a position and velocity
estimator for moving obstacles, and a motion planner to set the waypoint and
generate motion primitives. Considering the fast flight and the rapid change
of the environmental information, a safe and effective obstacle avoidance
algorithm is needed to process up-to-date information from the drones during
flight. Thus, the relative velocity [2] method is applied for static and
dynamic obstacle collision check. It is combined with our former proposed
heuristic angular search (HAS) [3] method to avoid both static and dynmaic
obstacles efficiently. With point cloud input, a cluster algorithm is adopted
to divide the point cloud into isolate obstacles, then the static and dynamic
obstacles can be sorted by the displacement between two sensor frames. A depth
camera is utilized to obtain the point cloud. To overcome its narrow field of
view (FOV), the static points are memorized to prevent the drone from hitting
obstacles that just moved outside the FOV. For moving obstacle avoidance, the
information (position, velocity and size) of the velocity obstacle is used to
choose a safe path and smaller acceleration for the drone. This is better than
considering all the points as static obstacles.

In summary, the main contributions of the paper are as follows:

  * •

A novel computational efficient framework is proposed in this paper for UAVs
navigation to avoid dynamic and static obstacles concurrently on point cloud
with a near optimal strategy.

  * •

We introduce the “spherical hypothesis” to reduce errors when estimating the
obstacle position and use a Kalman filter to make the estimation output more
accurate and stable. The position, size, and velocity of the moving obstacles
are estimated from the point cloud.

  * •

Based on the HAS and relative velocity method, a velocity planning algorithm
is designed to make the planner more effective and try to reduce the
acceleration cost while guaranteeing safety.

In addition, we conduct a number of simulation and hardware tests to prove the
proposed planner is effective. Furthermore, the computation time rarely
increases (only up about 5 ms) compared to the method which does not take into
account the velocity of obstacles [3].

##  II RELATED WORK

At present, there are many methods for path planning and obstacle avoidance in
static environments. Although some researchers have published their safe
planning framework for UAVs in an unknown static scenario, it is a more
complex problem for a UAV with a single depth camera flying in an unknown
environment with dynamic obstacles. Because the FOV is narrow and the pose of
the camera is unstable during the flight. For hardware experiments, it is
necessary to encode and use the information of detected obstacles in an
efficient way to make the drone react agilely to moving obstacles in advance.

For identifying moving obstacles from the environment, most researchers employ
the raw image from the camera and mark the corresponding pixels before
measuring the depth. The semantic segmentation network with moving consistency
check method based on images can distinguish the dynamic objects [4]. Also, a
block-based motion estimation method to identify the moving obstacle is used
in [5], but the result is poor if the background is complex. If only human is
considered as moving obstacle, the human face recognition technology can be
applied [6]. However, the above-mentioned works do not estimate the obstacle
velocity and position. [7] proposes such an approach, which jointly estimates
the camera position, stereo depth and object detections, and it can track the
trajectories. But all the above image-based method is computation-expensive,
not appropriate for the onboard micro computer. Based on the point cloud from
lidar, it is also possible to estimate the moving obstacles’ position and
velocity [8]. This inspired our own approach in this paper. Event cameras can
distinguish between static and dynamic objects and enable the drone to avoid
the dynamic ones in a very short time [9]. However, the event camera is
expensive for low-cost UAVs and not compatible to sense the static obstacles.

In terms of the avoidance of moving obstacles for navigation tasks, the
majority of research works are based on the applications of ground vehicles.
The forbidden velocity map [2] is designed to solve out all the forbidden 2D
velocity vectors and they are represented as two separate areas in the map.
The artificial potential field (APF) method can avoid the moving obstacles by
considering their moving directions [10]-[11]. Also, the model predictive
control (MPC) method is tested, but the time cost is too large for real-time
flight. [12] proposes the probabilistic safety barrier certificates (PrSBC) to
define the space of admissible control actions that are probabilistically
safe, which is more compatible for multi-robot systems. [13] adopts the
similar relative velocity concept with this paper, while [13] is in the 2D
scenario and it samples from the velocity space rather than find the optimal
solution.

For generating motion primitives and predicting the trajectory, one typical
solution is to build a local occupancy grid map with the most recent
perception data and generate a minimum-jerk trajectory through the waypoints
[14]-[15]. A bi-level optimization was used in [16]-[17] to find the time
constrains for motion optimization. Additionally, the motion primitives can be
obtained by solving an optimization problem directly without searching for the
waypoints [18]-[20]. Usually, a Bezier curve is used to express the predicted
trajectory for its convex property. Another way is to directly generate motion
primitives by sampling [21], then select the most suitable group of motion
primitives by an evaluation function as the output. One representative work is
[22], the drone can be precisely controlled to fly an intersecting trajectory
with a falling ball and catch it. Except the sampling-based methods, it is
difficult to significantly increase the calculation speed within their own
framework.

##  III TECHNICAL APPROACH

TABLE I: definition of mathematical symbols $\overline{\quad},\ \overrightarrow{}$ |  the line segment and vector between the two points |  $Pcl_{t_{1}},Pcl_{t_{2}}$ |  the former and latest frames of $Pcl_{2}$, at timestamp $t_{1}$ and $t_{2}$
---|---|---|---
$p_{ali},\ e_{ali}$ |  the UAV position & orientation aligned to $Pcl_{2}$ |  $p_{t_{1}},\ p_{t_{2}}$ |  position of the UAV at timestamp $t_{1}$ & $t_{2}$
$e_{t_{1}},\ e_{t_{2}}$ |  orientation of the UAV at timestamp $t_{1}$ & $t_{2}$ |  $OT_{1}$, $OT_{2}$ |  lists for point clusters and their centers in $Pcl_{t_{1}}$ and $Pcl_{t_{2}}$
$C_{1}$, $C_{2}$ |  lists for clusters centers in $OT_{1}$ and $OT_{2}$ |  $ot_{1}$, $c_{1}$ |  any point cluster in $OT_{1}$, and the corresponding center
$len()$ |  function returns the length of the input list |  $D(),\ V()$ |  return the points coordinate variance and obstacle volume
$C_{M}(),C_{V}()$ |  return the mean and variance of the points color value |  $C_{d}$ |  the list for storing the moving obstacle position $c_{2k}$
$R,\ H,\ Q$ |  observation noise covariance, observation, and error matrix |  $P_{t}$ |  posterior estimation error covariance
$\hat{c}_{2k}$, $\hat{v}_{2k}$ |  filtered position and velocity of $ot_{2k}$ |  $a_{t-1}$ |  observed obstacle acceleration
$p_{n},\ v_{n},\ a_{n}$ |  the position, velocity and acceleration of UAV for motion planning at current step |  $abs()$ |  returns the vector composed of the absolute values of all the input vector elements
$ag()$ |  returns the direction angle of the given vector |  $rd()$ |  returns the closest integer of the input number
$\alpha_{0}$ |  the direction angle of the original relative velocity |  $\hat{C}_{d}$ |  the list storing all $\hat{c}_{2k}$ computed by the Kalman filter
$r_{obs}$ |  the size of the moving obstacle |  $r_{safe}$ |  safety radius, a pre-assigned constant
$w_{p(n)}$ |  the point where the planned trajectory try to end |  $argmin()$ |  returns the label of the minimum element in a vector
$A_{t}$ |  matrix to solve $s_{p}$ |  $v_{unit}$ |  unit vector for the current search direction of HAS
$s_{p}(1)$, $s_{p}(2)$ |  magnitudes of velocity and relative velocity planned for the drone next step |  $V_{g},\ v_{g}$ |  the list of all the feasible UAV velocity magnitudes in the next step, and the finally choosen one in $V_{g}$
$\xi$ |  tolerance of the difference between the end of the predicted trajectory and next waypoint |  $t_{max}$ |  upper bound of the time which can be used to finish the predicted piece of trajectory
$a_{np},\ t_{np}$ |  the acceleration and time variables for the optimization |  $v_{n+1},\ p_{n+1}$ |  the predicted UAV velocity and position at the next step
$v_{min}$, $v_{max}$, $a_{max}$ |  kinematic constraints for speed and acceleration |  $p_{0},\ e_{0}$ |  the position & orientation message output by the buffer
$v_{p},\ v_{e}$ |  the UAV translational and angular velocity for alignment |  $t_{c},\ t_{p}$ |  timestamps of $Pcl_{2}$ and the pose message of UAV
$a_{p}$, $a_{e}$ |  the UAV translational and angular acceleration for alignment |  $\Delta t_{n}$ |  average time cost of the motion planner loops
$t_{pm}$ |  timestamp gap between UAV pose and current time |  $t_{dp}$ |  timestamp gap between UAV pose and $Pcl_{2}$
$t_{pl}$ |  time cost of the former step of the motion planner |  $t_{ct}$ |  fixed time cost for the flight controller

Our proposed framework is composed of three submodules that run parallelly and
asynchronously: the point cloud filter (section IV.A), the moving obstacle
position and velocity estimator (section III.A & B), and the motion planner
(section III.C & D). Their execution frequency is different, and the alignment
for the message conveying between them is discussed in section IV. Table I
explains the definitions of part of the symbols in this paper, other symbols
are explained in (or near) the figures and algorithms for reading convenience.

###  III-A Classification for Static and Dynamic Obstacle

First, the raw point cloud $Pcl_{1}$ is filtered to obtain $Pcl_{2}$,
$p_{ali}$ and $e_{ali}$ are aligned to the timestamp of $Pcl_{2}$. $Pcl_{2}$
is the filtered point cloud in earth coordinate $E\\!-\\!X\\!Y\\!Z$, the
details about the filter are in section IV. The next step is to sort the
obstacles by comparing two $Pcl_{2}$ frames at different moments and estimate
the displacement of the obstacles. The detailed procedure is introduced in
Algorithm 1. Its time complexity is $O(NlogN)$ to $O(N^{2})$ and space
complexity is $O(N)$. $N$ indicates the size of $Pcl_{2}$. First, for all the
useful data, two data frames at the the former timestamp $t_{1}$ and latest
timestamp $t_{2}$ is prepared (Algorithm 1, Line 2-3). To deal with the
movement of the camera between the two frames, the core idea is filtering
$Pcl_{t_{2}}$, only keeping the points within the FOV of the camera at $t_{1}$
(Line 4). So that the two point cloud frames can be regarded as from one
static camera. It is important for the obstacle matching. The newly appeared
obstacles in the latest frame are removed, so only the obstacles appear in
both of the two point cloud frames are further analyzed. The minimal time
interval $t_{2}-t_{1}$ is set to be able to make the displacement of the
dynamic obstacles obvious enough to be observed, while maintaining a
reasonable output frequency of the estimation results.

Next, the frame from $Pcl_{2}$ is separated into several clusters with each
cluster representing one obstacle (Line 5). Density-based spatial clustering
of applications with noise (DBSCAN) [23] can cluster the points into different
obstacles by the density of the points and no pre-assigned cluster amount is
required. The position of each obstacle is estimated by the spherical
hypothesis, which is introduced in the next section.

Then, the same obstacle in the two frames should be matched. This is matched
by comparing the feature vector $fte()$ of two clusters, which is defined in
(1). The feature vector contains six features that can be extracted from the
point cloud, which are easy to calculate and proved to be very effective in
tests. The idea is: if there is not a significant difference in the position,
shape, and color of the two point clusters extracted from two point cloud
frames respectively, then they are commonly believed the same object.

$fte(ot_{1})=[c_{1},len(ot_{1}),D(ot_{1}),V(ot_{1}),C_{M}(ot_{1}),C_{V}(ot_{1})]$
$None$

Finally, the moving obstacles are detected by comparing the obstacle center
displacement with the threshold $d_{s}$ (Algorithm 1, line 8-9). $d_{Ecd}$ is
the threshold for the feature vector difference, $d_{fte}<d_{Ecd}$ stands for
that the same obstacle is matched. The points of the static obstacle are
stored in a memory list $Pcl_{s}$ with a fixed size, to remember the static
obstacles that just move out the FOV. $C_{d}$ is cleared at the beginning of
each loop, while $Pcl_{s}$ not.

Algorithm 1 Sorting the static and dynamic obstacles

1: while true: do

2: Record $Pcl_{2}$, $p_{ali}$, $e_{ali}$ with their timestamps into separate
lists

3: Extract
$Pcl_{t_{1}}$,$Pcl_{t_{2}}$,$p_{t_{1}}$,$p_{t_{2}}$,$e_{t_{1}}$,$e_{t_{2}}$ at
timestamp $t_{1},t_{2}$ $(t_{2}-t_{1}>d_{t})$ from the lists built in line 2

4: Remove points in $Pcl_{t_{2}}$ which is out of the FOV of camera at the
pose at $t_{1}$

5: Cluster $Pcl_{t_{1}},Pcl_{t_{2}}$ with DBSCAN to obtain the clusters
$OT_{1}(ot_{11},ot_{12},...)$, $OT_{2}(ot_{21},ot_{22},...)$

6: Calculate the corresponding obstacle position (section IV.B), get
$C_{1}(c_{11},c_{12},...)$ and $C_{2}(c_{21},c_{22},...)$

7: for $ot_{2k}$ in $OT_{2}$ ($k$ is the iteration number): do

8: Calculate the Euclidean distance $d_{fte}=\|fte(ot_{1})-fte(ot_{2k})\|_{2}$
with all $OT_{1}$ and $ot_{2k}$, $ot_{1j}$ ($j$ is the index) in $OT_{1}$ is
corresponding to the minimal distance, $d_{kj}=\overline{c_{2k}c_{1j}}$.

9: if $d_{kj}<d_{s}$ and $d_{fte}<d_{Ecd}$: then

10: Add $ot_{2k}$ to list $Pcl_{s}$

11: else

12: Add $c_{2k}$ to list $C_{d}$

13: end if

14: end for

15: end while

###  III-B Obstacle Position and Velocity Estimation

The velocity $v_{2k}$ of the obstacle $ot_{2k}$ can be obtained by
$v_{2k}=\overrightarrow{c_{2k}c_{1j}}/{(t_{2}-t_{1})}$. Besides, a Kalman
filter in (2) is adopted to further reduce the position and velocity
estimation error and make the estimating output denser. The superscript -
indicates the prior state matrix $\hat{x}_{t}$ and the error covariance matrix
$P_{t}$ before updated by the Kalman gain matrix $K_{t}$. $F_{t}$ is the state
transition matrix and $B_{t}$ is the control matrix. $x_{t}$ is composed of
the obstacle position and velocity before the filtering, and $\hat{\ }$ marks
the filtered results for $x_{t}$. $\hat{x}_{t}$ equals to the predicted state
$\hat{x}_{t}^{-}$ if no dynamic obstacle is caught by Algorithm 1. The
subscripts $t$ and ${t\\!-\\!1}$ distinguish the current and former step of
the Kalman filter. $\Delta t$ is the time interval between each run of the
Kalman filter. On the current stage, we assume the moving obstacle performs
uniform motion between $t_{1}$ and $t_{2}$, $a_{t-1}=0$.

$\begin{array}[]{c}\hat{x}_{t}^{-}=F_{t}\hat{x}_{t-1}+B_{t}a_{t-1},\quad
P_{t}^{-}=F_{t}P_{t-1}F_{t}^{T}+Q\\\
K_{t}=P_{t}^{-}H^{T}\left(HP_{t}^{-}H^{T}+R\right)^{-1},\quad
P_{t}=\left(I-K_{t}H\right)P_{t}^{-}\\\
\hat{x}_{t}=\left\\{\begin{array}[]{l}\hat{x}_{t}^{-}+K_{t}\left(x_{t}-H\hat{x}_{t}^{-}\right)\
\text{($C_{d}$ is not empty)}\\\ \hat{x}_{t}^{-}\ \text{($C_{d}$ is
empty)}\end{array}\right.\\\ x_{t}=\left[\begin{array}[]{l}c_{2k}\\\
v_{2k}\end{array}\right],F_{t}=\left[\begin{array}[]{cc}1&\Delta t\\\
0&1\end{array}\right],B_{t}=\left[\begin{array}[]{c}\frac{\Delta t^{2}}{2}\\\
\Delta t\end{array}\right]\end{array}$ $None$

###  III-C Collision check and velocity planning with the relative velocity
method

####  III-C1 Review of the HAS method

To help understand Algorithm 2, our previously proposed HAS method [3] is
briefly revisited as follows. In HAS, a waypoint $w_{p(n)}$ close to the drone
is first searched as the trajectory endpoint constrain for the motion
planning. Moreover, the real trajectory between $p_{n}$ and $w_{p(n)}$ is
proved collision-free. As shown in Fig. 2, a bunch of line segments spread out
from the initial search direction $A_{g0}$, and these line segments have a
common start point $p_{n}$ and the same length $d_{use}$. $d_{use}$ is the
point cloud distance threshold, the points whose distance from $p_{n}$ further
than $d_{use}$ are not considered in the collision check. The two line
segments of symmetry about $A_{g0}$ on the plane parallel to the ground plane
are first checked if they collide with obstacles. If they collide, the two
lines in the vertical plane will be checked. These four lines have the same
angle difference with $A_{g0}$. If the minimal distance between the line and
obstacles is smaller than $r_{safe}$, it is treat as collision. Fig. 2 shows
when the first round of search has failed, another round with greater angle
difference is conducted until a collision-free direction
$\overrightarrow{p_{n}p_{di}}$ is found. $w_{p(n)}$ is on
$\overrightarrow{p_{n}p_{di}}$ and $\|p_{n}w_{p(n)}\|_{2}$ should satisfy the
safety analysis in [3].

Figure 2: The illustration of HAS process. The red dash circle and red arrows
are for the search on the plane parallel to ground, while the green ones are
for the vertical plane.

####  III-C2 Collision check and velocity planning

Figure 3: The graph illustration for the relative velocity method. To be
clear, only one moving obstacle is shown in the figure (a). $v_{t}$ is the
obstacle velocity. At first $d_{pfi}<r_{obs}+r_{safe}$. After Algorithm 2, the
planned velocity magnitude $v_{g}$ is determined. “p.r.v.d” stands for the
finally planned relative velocity direction. The distance from $p_{n}$ to
p.r.v.d equals $r_{obs}+r_{safe}$. It is also safe if the drone speed is
greater than $v_{g}$. Figure(b) depicts the two return components
$\alpha_{g1}$ and $\alpha_{g2}$ of function ag(), i.e.,
$ag()=(\alpha_{g1},\alpha_{g2})$. Algorithm 2 collision check and velocity
planning

1: Run Algorithm 4 (HAS) in [3] with $Pcl_{s}$, find the collision-free
$\overrightarrow{p_{n}p_{di}}$ for static obstacles.

2: for $c_{w}$ in $\hat{C}_{d}$ ($w$ is the iteration number): do

3:
$d_{pfi}=\dfrac{\|\overrightarrow{p_{n}c_{w}}\times\overrightarrow{c_{w}p_{fi}}\|_{2}}{\left\|\overrightarrow{p_{n}p_{fi}}\right\|_{2}}$

4: if $d_{pfi}<r_{obs}+r_{safe}$ $\wedge$ $\angle c_{w}p_{n}p_{fi}<90^{\circ}$
then

5:
$\alpha_{obs}=arcsin\left(\dfrac{r_{obs}+r_{safe}}{\left\|\overrightarrow{p_{n}c_{w}}\right\|_{2}}\right)$

6:
$\Gamma(m+1)=(rd((4-m)/4),rd(m/4))\alpha_{obs}+ag(\overrightarrow{p_{n}c_{w}})$,
$m=\\{0,1,2,3\\}$, $\alpha_{0}=ag(\overrightarrow{p_{n}p_{fi}})$

7:
$\gamma=\Gamma(argmin(\|\Gamma(1)-\alpha_{0}\|_{2},...,\|\Gamma(4)-\alpha_{0}\|_{2}))$

8: $s_{p}=v_{t}\cdot A_{t}$

9: if $\|v_{unit}s_{p}(2)-v_{n}\|_{2}<\Delta t_{n}a_{max}$ then

10: Add $s_{p}(2)$ to list $V_{g}$

11: else

12: Break the circle

13: end if

14: end if

15: end for

16: if $V_{g}$ is not empty then

17: Convert $V_{g}$ to vector, $v_{g}=V_{g}(argmin(abs(V_{g}-\|v_{n}\|_{2})))$

18: end if

$v_{unit}=\dfrac{\overrightarrow{p_{n}p_{di}}}{\left\|\overrightarrow{p_{n}p_{di}}\right\|_{2}},A_{t}=\left[\begin{array}[]{cc}cos(\gamma(1))&v_{unit}(1)\\\
sin(\gamma(1))&v_{unit}(2)\\\ tan(\gamma(2))&v_{unit}(3)\end{array}\right]$
$None$

Algorithm 2 reveals the process of searching for the next desired velocity for
avoiding multiple moving obstacles. Its time and space complexity are both
$O(N)$. (3) gives the definition for $v_{unit}$ and $A_{t}$. $v_{s-p}$ is the
original planned velocity if only HAS is used, $v_{s-p}$ is parallel to
$\overrightarrow{p_{n}p_{di}}$ and $\|v_{s-p}\|_{2}=\|v_{n}\|_{2}$.
$\overrightarrow{p_{n}p_{fi}}$ is parallel to the relative velocity
$(v_{s-p}-v_{t})$. Fig. 3(a) shows the situation when the planned velocity
$v_{g}$ is found by Algorithm 2.

At the beginning, the original collision check algorithm of HAS method [3] is
used, and Algorithm 2 mainly introduces the specific process of collision
check based on the relative velocity method. The direction angle function
$ag()$ is explained in Fig. 3(b), it returns a 2d vector. If $v_{s-p}$ is not
safe ($d_{pfi}<r_{obs}+r_{safe}$), we calculate the direction angle of
$\overrightarrow{p_{n}c_{w}}$, and add $\alpha_{obs}$ from four orthogonal
directions on it to obtain 4 feasible directions of the relative velocity
(FDRV). $\alpha_{obs}$ is the minimum angle increment to ensure the FDRV keeps
a safe distance from the obstacle $c_{w}$. $\angle
c_{w}p_{n}p_{fi}<90^{\circ}$ is to ignore the obstacles who are “behind” the
drone because they have no collision risk. $\Gamma$ is the list for storing
the direction angle. Line 7 finds the FDRV with the minimal acceleration cost
for the current obstacle $c_{w}$, denoted as $\gamma$. Because only 2 vertical
and 2 horizontal directions are considered, the acceleration to reach the
planned velocity is only near optimal. Line 9 checks if the desired speed can
be reached within the acceleration constrain. Finally, Line 17 finds $v_{g}$
with the minimal acceleration from all the feasible velocity magnitudes. If
$v_{g}$ can not be found after Algorithm 2, the current search direction
$\overrightarrow{p_{n}p_{di}}$ fails, and Algorithm 2 starts with the next
search direction in HAS. If there is no moving obstacle, $\|v_{g}\|_{2}$
defaults to equal $v_{max}$.

In conclusion, Algorithm 2 calculates the end of the speed range for the drone
at the next step for the motion optimization. It is a more precise and energy-
saving way to control the drone rather than suddenly brake down or accelerate.
The safe speed range is introduced in section III.D. In most cases of
simulation tests, Algorithm 2 can be done within 9 ms on average.

###  III-D Motion Planning

After obtaining the waypoint $w_{p(n)}$ and desired velocity magnitude
boundary $v_{g}$, the motion primitives are calculated and sent to the flight
controller. The optimization problem to obtain motion primitives is defined in
(4). $v_{n+1}$ and $p_{n+1}$ are calculated by the kinematic formula.
$\eta_{1},\ \eta_{2}$ are coefficients, the default values are shown in Table
II.

The constraints and the objective function are different by whether the
dynamic obstacle exists. If there exists a moving obstacle, $\xi=\infty$ to
convert the trajectory end point constrain to the component of object
function. Otherwise $\eta_{2}=0$ to strictly constrain the trajectory end
point. If $\|v_{g}\|_{2}<\|v_{n}\|_{2}$ then $v_{max}=\|v_{g}\|_{2}$,
otherwise $v_{min}=\|v_{g}\|_{2}$. This is the safe speed range, it decides
the drone should whether decelerate or accelerate according to the planned
speed. In any above conditional equation, the parameter not mentioned equals
to the default value given in Table II. The constraint $a_{max}$ ensures the
aircraft can fly within its own kinematic limit. The object function is
designed to solve the minimum acceleration and the shortest time cost to reach
the next waypoint and desired velocity. By adjusting $\eta_{1}$, the flight
pose stationarity and the speed can be balanced. The drone will fly more
aggresively if we increase $\eta_{1}$.

$\displaystyle\min_{a_{np},t_{np}}\left\|a_{np}\right\|_{2}+\eta_{1}t_{np}+\eta_{2}\|\overrightarrow{p_{n+1}w_{p(n)}}\|_{2}$
$None$ s.t. $\displaystyle 0<\mathrm{t}_{np}\leq t_{\max},\
v_{min}\leq\|v_{n+1}\|_{2}\leq v_{max}$
$\displaystyle\left\|a_{np}\right\|_{2}\leq a_{\max},\
\|\overrightarrow{p_{n+1}w_{p(n)}}\|_{2}\leq\xi$ $\displaystyle
v_{n+1}=v_{n}+a_{np}t_{np},\
p_{n+1}=p_{n}+v_{n}t_{np}+\frac{1}{2}a_{np}t_{np}^{2}$

##  IV IMPLEMENTATION OF THE METHODS

This section introduces the technique details for implementing and improving
our proposed method.

###  IV-A Raw point cloud filtering

In this paper, filtering the raw point cloud is necessary, because the
obstacle estimation is sensitive to the noise. The noise should be eliminated
strictly, even losing a few true object points is acceptable. The filter has
the same structure with our former work [3], as shown in Fig. 4, but the
parameters are different. $Pcl_{11}$, $Pcl_{12}$, $Pcl_{13}$ are the output of
the three filters in Fig. 4 repectively. Distance filter removes the points
too far ($\geq 4\ m$) from the camera, voxel filter keeps only one point in
one fixed-size ($0.2\ m$) voxel, outlier filter removes the point that does
not have enough neighbors ($\leq$ 13) in a certain radius ($0.3\ m$). These
metrics are tuned manually during extensive tests on the hardware platform
introduced in section V, to balance the point cloud quality and the depth
detect distance. They are proved satisfactory for obstacle position
estimation. The point cloud filtering also reduces the message size by one to
two orders of magnitude, so the computation efficiency is much improved, while
the reliability of the collision check is not affected.

However, when the drone performs an aggressive maneuver, the motion blur of
the camera may cause the wrong point cloud output, which may fail the
collision-free waypoint generation. To solve this problem, we propose a
practical and effective measurement: Only the filtered point cloud $Pcl_{2}$
is accepted when the angular velocity of the three Euler angles of the drone
is within the limit $\omega_{max}$.

In addition, before Algorithm 1 is executed, the pose feedback message from
the UAV controller should align with the timestamp on $Pcl_{2}$. It is to
convert $Pcl_{13}$ in the body coordinate $B\\!-\\!xyz$ more precisely to
$Pcl_{2}$ in the earth coordinate $E\\!-\\!X\\!Y\\!Z$. So a message buffer is
set to output the aligned point cloud and UAV pose message within a short time
interval (i.e. $\leq 0.03\ s$). After that the pose of UAV at the timestamp of
$Pcl_{2}$ is further estimated in (5) by kinematics formulation. In (5) we
assume that the translational and rotational acceleration are constant during
time ${t}_{c}\\!-\\!{t}_{p}$, the accuracy is satisfactory when
${t}_{c}\\!-\\!{t}_{p}$ is small enough.

Figure 4: The filtering process from $Pcl_{1}$ to $Pcl_{2}$

$\left[\begin{array}[]{c}{p}_{ali}\\\
{e}_{ali}\end{array}\right]=\left[\begin{array}[]{c}{p}_{0}\\\
{e}_{0}\end{array}\right]+\left[\begin{array}[]{c}{v}_{p}\\\
{v}_{e}\end{array}\right]\left({t}_{c}-{t}_{p}\right)+\dfrac{1}{2}\left[\begin{array}[]{c}{a}_{p}\\\
{a}_{e}\end{array}\right]\left({t}_{c}-{t}_{p}\right)^{2}$ $None$

###  IV-B Spherical hypothesis

The point cloud only describes a part of the obstacle which is facing the
camera, as shown in Fig. 5(a). $v_{obs}$ is the real velocity of the obstacle,
while $v^{\prime}_{obs}$ is the estimated velocity with the point cloud
center. The point cloud center position has obvious difference between the
real obstacle center, so it can not be used as the obstacle position. Thus,
the spherical hypothesis is proposed to reduce the error by supposing the
obstacle as a sphere. In Fig. 5(b), $p_{c}$ is the center of the sphere,
$p_{g}$ is the point cloud center and its coordinate is known. $p_{m}$ is the
farthest point in the point cloud from $p_{g}$. $\overrightarrow{p_{n}p_{m}}$
is the tangent of the circle. $r_{obs}=\overline{p_{c}p_{m}}$ is the radius of
the sphere. The coordinate of $p_{c}$ can be solved by (6), it is the obstacle
position used in Algorithm 2.

Figure 5: The error in estimating the obstacle position and the spherical
hypothesis. The real situation is 3D. In order to facilitate the presentation
of the concept, it is put on a floor plan, and it is the same for Fig. 3.

$\displaystyle\alpha=arccos(\dfrac{\overline{p_{m}p_{n}}^{2}+\overline{p_{g}p_{n}}^{2}-\overline{p_{m}p_{g}}^{2}}{2\overline{p_{m}p_{n}}\cdot\overline{p_{g}p_{n}}})$
$None$ $\displaystyle\beta=\dfrac{\pi}{2}-\alpha\text{ ,
}\overline{p_{c}p_{m}}=\overline{p_{m}p_{n}}tan(\alpha)$
$\displaystyle\overline{p_{c}p_{g}}=\overline{p_{c}p_{m}}\dfrac{sin(\beta)}{\beta}\text{
,
}p_{c}=p_{n}+\overrightarrow{p_{g}p_{n}}\dfrac{\overline{p_{c}p_{n}}}{\overline{p_{g}p_{n}}}$

Although the spherical hypothesis also causes errors because an obstacle in
real-life is rare to be a perfect sphere, it is reliable in safety. The
estimation errors of the obstacle position and radius will not harm the safety
with the relative velocity method. Intuitively, because the backside of an
obstacle is not considered in the collision check, only the part can be seen
by the camera matters the collision check [3]. In Fig. 5, the projected area
of the proposed sphere on camera is not smaller than the point cloud. Thus, it
is safe to replace the point cloud with this sphere.

###  IV-C Position estimation for deformable obstacles

Here we introduce another technique for reducing the position estimation error
of obstacle if the obstacle deforms during its traveling, such as a walking
human. The waving limbs of a walking human often increase the human position
estimation error due to the point cloud deformation. But when two neighbor
frames of $Pcl_{2}$ are overlaid (i.e. $Pcl_{t_{2}}=Pcl_{t_{2}}\cup
Pcl_{t_{2}-b}$, the subscript -b refers to the former neighboring message),
the point cloud of the human trunk is denser than the other parts which rotate
over the trunk. Then increasing the point density threshold of DBSCAN can
remove the points corresponding to the limbs. The mean of $(p_{t_{2}},\
p_{t_{2}-b})$ is used to replace $p_{t_{2}}$. For example
$p_{t_{2}}=(p_{t_{2}}+p_{t_{2}-b})/2$, and it is the same for
$p_{t_{1}},e_{t_{1}}$ and $e_{t_{2}}$ (Algorithm 1, line 3).

###  IV-D Position prediction in the planner

The motion of the drone is more aggressive for avoiding moving obstacles than
flying in a static environment. To address the displacement of the drone
during the time costed by the trajectory planner and flight controller, time
compensation is adopted before implementing Algorithm 2. The current position
of drone $p_{n}$ is updated by the prediction, as shown in (7). In addition,
due to the time cost of obstacle identification, the timestamp on the
information of dynamic obstacles is always delayed than that of the pose and
velocity message of the drone. Based on the uniform acceleration assumption,
the obstacle position coordinate $\hat{c}_{2k}$ at the current time is
predicted and updated with the time gap $t_{dp}$ as (8).

$p_{n}=p_{n}+(t_{pl}+t_{ct}+t_{pm})v_{n}+\frac{1}{2}(t_{pl}+t_{ct}+t_{pm})^{2}a_{n}$
$None$ $\hat{c}_{2k}={\hat{c}}_{2k}+(t_{pl}+t_{ct}+t_{pm}+t_{dp})\hat{v}_{2k}$
$None$

##  V EXPERIMENTAL RESULTS

###  V-A Experimental Configuration

The detection and avoidance of obstacles are tested and verified in the Robot
Operation System (ROS)/Gazebo simulation environment first and then in the
hardware experiment. The drone model used in the simulation is 3DR IRIS, and
the underlying flight controller is the PX4 1.10.1 firmware version. The depth
camera model is Intel Realsense D435i with resolution 424*240 (30 fps). For
hardware experiments, we use a self-assembled quadrotor with a QAV 250 frame
and an UP board with an Intel Atom x5 Z8350 1.44 GHz processor, other
configuration keeps unchanged. Table II shows the parameter settings for the
tests. The supplementary video for the tests has been uploaded
online111https://youtu.be/TgHY7i5K6v8.

TABLE II: parameters for the simulation Parameter | Value | Parameter | Value | Parameter | Value
---|---|---|---|---|---
$d_{use}$ | 3 m | $t_{ct}$ | 0.01 s | $a_{max}$ | 4 m/s2
$t_{max}$ | 1.2 s | $v_{max}$ | 3 m/s | $r_{safe}$ | 0.5 m
$\omega_{max}$ | 1.5 rad/s | $\overline{p_{n}w_{p(n)}}$ | 0.3m | $t_{2}-t_{1}$ | 0.2 s
$\eta_{1}$ | 6 | $\eta_{2}$ | 20 | $\xi$ | 0.02

###  V-B Simulation Test

First, the accuracy and stability of the estimation method for the obstacle
position and velocity are verified.

In the simulation world as depicted in Fig. 6(a), there are one moving ball,
two moving human models and some static objects. The camera is fixed on the
head of the drone, facing forward straightly. The drone is hovering around the
point $(-6,0,1.5)$ in the range of 1 $m$ to keep the camera moving. Fig. 6(b)
depicts the visualized estimation results in Rviz. The green spheres indicate
moving obstacles, the red arrows represent the obstacle velocity. The
estimation numeral results are shown in Fig. 7. We compare them with the
ground truth and the estimation results without the Kalman filter. Fig. 7(a)
exhibits that the Kalman filter significantly improves the accuracy. The
average velocity estimation error is reduced from $\pm 0.49\ m/s$ to $\pm
0.12\ m/s$, and the original data develops from sparse into dense. In Fig.
7(b), the dash and solid lines represent the estimated position and the ground
truth of it. The average position estimation error for the moving person (0.31
m) is larger than the moving ball (0.12 m) due to body posture changes. The
estimation test results demonstrate that our estimation algorithm is practical
for dynamic obstacle avoidance, the velocity estimation relative error is only
about $10\%$.

Fig. 8(a) is the overview of the simulation world. In Fig. 1, the better
safety of our proposed method is illustrated: To avoid the moving man which is
at a similar speed with the drone, the aircraft choose to fly in the
“opposite” direction with the man so the threat is removed easily. If only the
static HAS method is utilized in the same situation, the drone decelerates and
fly alongside the man(red line), which is very inefficient. Fig. 8(c) is the
visualized data in RVIZ to show different motion choices in an obstacle
avoidance procedure with and without the relative velocity method (the left
two & the right figure). The data is processed after the flight simulation to
calculate the relative velocity and draw it on the picture accordingly. The
red straight line connects the goal and $p_{n}$, and the green line is the
real path updated at 1 Hz. For the two left figures in Fig. 8(c), the drone
detects a left-moving obstacle at its forward right side first. The drone
chooses to decelerate to avoid it, but finally the drone finds the obstacle
moves slower than what it was observed before. So the best choice is to move
right, i.e., in the opposite moving direction of the obstacle. For the right
figure, the drone fly left to choose the shortest path to pass, considering
the person as static. But it probably leads to a collision, because the drone
and person go in the same direction. All in all, with the relative velocity
method, the orientation of relative velocity always keeps a safe distance from
the moving obstacles.

Figure 6: The simulation environment for the moving obstacles’ position and
velocity estimation test. The colorful points stand for static obstacles.

Figure 7: The estimation results of the moving obstacles’ velocity and
position

Figure 8: (a): The dynamic simulation test environment. (b): The dynamic
hardware test environment. (c): the data in RVIZ corresponding to Fig. 1. (d):
The composed picture of the hardware flight test. Numbers in (d) mark the same
frame for the drone and person.

###  V-C Hardware Test

We set up a hardware test environment as Fig. 8(b), the drone takes off behind
the boxes and then a person enters the FOV of the camera and walks straight
after the drone passes static boxes to test the effectiveness of our method.
In Fig. 8(d), the camera is fixed and takes photos every 0.7 s during the
flight. Seven photos are composed together. In the 4th frame the walking
person appeared, the green line shows the trajectory of the drone while the
blue dash line is for the person. It can be concluded that the reaction of the
drone is similar to the simulation above. The data for this hardware test is
shown in Fig. 9, the velocity curve indicates that the drone reacts very fast
once the moving obstacle appears. To analyze the change of time cost ,
$Pcl_{s}$ only use the latest data frame so its size varies. In Fig. 9(b), the
blue line is only for line 1 in Algorithm 2 (static collision check) and the
red line represents the whole Algorithm 2. The blue line has a strong positive
linear correlation with time, because the number of the input points of the
collision check procedure determines the distance calculation times. While the
red and yellow line shows the irregularity, because the moving obstacle brings
external computation burden to Algorithm 2, and the number of moving obstacles
has no relation with the point cloud size.

At last, we compare our work with SOTA in Table III. Since all related works
differ significantly from ours in terms of application background and test
platform, for numeral indicators we only compare the time cost for reference.
The abbreviations stand for: obs (obstacle), atc (average time cost).
“Optimality” refers to if the trajectory planner considers the minimal
acceleration or the shortest trajectory to avoid the moving obstacle. “N/A”
refers to the work that gets obstacle information from an external source and
does not include obstacle detection procedure. Most works have severe
restrictions on the obstacle type, and do not consider the optimality. Our
work has a great advantage in generality and optimality, the time cost is also
the shortest unless the event camera is considered.

Figure 9: (a): The velocity of the drone in earth coordinate $E\\!-\\!X\\!Y\\!Z$. (b): The time cost for different modules under different point number of $Pcl_{s}$. The average value is marked in the corresponding color. TABLE III: comparison between different works Work | Sensor type | Obs type limits | Time cost | Optimality
---|---|---|---|---
[13] | Sonar | dynamic obs | 1-2 s | No
[10] | N/A | dynamic obs | 0.045-0.13 s | No
[8] | Camera&Lidar | No | About 0.1 s | No
[6] | N/A | human | 0.2-0.3 s | No
[9] | Event camera | dynamic obs | 3.5 ms | No
Ours | Depth camera | No | Atc$\leq$0.02 s | Yes

##  VI CONCLUSION AND FUTURE WORK

In this paper, we present a computationally efficient algorithm framework for
both static and dynamic obstacle avoidance for UAVs based only on point cloud.
The test results indicate our work is feasible and show great promise in
practical applications.

However, when the speed or angular velocity of drone is high, and also because
of narrow FOV of single-camera, recognizing moving obstacles becomes
significantly difficult and unreliable. In future research, we intend to
improve the robustness of our method in aggressive flights and test it with
different sensors such as lidar.

## References

  * [1] J. Chen, T. Liu, and S. Shen, “Online generation of collision-free trajectories for quadrotor flight in unknown cluttered environments,” in 2016 IEEE International Conference on Robotics and Automation (ICRA),vol. 2016, 2016, pp. 1476–1483.
  * [2] B. Damas and J. Santos-Victor, “Avoiding moving obstacles: the forbidden velocity map,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009, pp. 4393–4398.
  * [3] H. Chen and P. Lu, “Computationally efficient obstacle avoidance trajectory planner for uavs based on heuristic angular search method,” IROS 2020, 2020. arXiv:2003.06136.
  * [4] C. Yu, Z. Liu, X.-J. Liu, F. Xie, Y. Yang, Q. Wei, and Q. Fei, “Ds-slam: A semantic visual slam towards dynamic environments,” in 2018IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2018, pp. 1168–1174.
  * [5] J. Kim and Y. Do, “Moving obstacle avoidance of a mobile robot using a single camera,” Procedia Engineering, vol. 41, pp. 911–916, 2012.
  * [6] T. Nageli, J. Alonso-Mora, A. Domahidi, D. Rus, and O. Hilliges, “Real-time motion planning for aerial videography with real-time with dynamic obstacle avoidance and viewpoint optimization,” in IEEE Robotics and Automation Letters, vol. 2, no. 3, 2017, pp. 1696–1703.
  * [7] A. Ess, B. Leibe, K. Schindler, and L. van Gool, “Moving obstacle detection in highly dynamic scenes,” in 2009 IEEE International Conference on Robotics and Automation, 2009, pp. 4451–4458.
  * [8] A. Cherubini, F. Spindler, and F. Chaumette, “Autonomous visual navigation and laser-based moving obstacle avoidance,” IEEE Transactions on Intelligent Transportation Systems, vol. 15, no. 5, pp. 2101–2110, 2014.
  * [9] D. Falanga, K. Kleber, and D. Scaramuzza, “Dynamic obstacle avoidance for quadrotors with event cameras.” Science Robotics, vol. 5,no. 40, 2020.
  * [10] N. Malone, H.-T. Chiang, K. Lesser, M. Oishi, and L. Tapia, “Hybrid dynamic moving obstacle avoidance using a stochastic reachable set-based potential field,” IEEE Transactions on Robotics, vol. 33, no. 5,pp. 1124–1138, 2017.
  * [11] H. Febbo, J. Liu, P. Jayakumar, J. L. Stein, and T. Ersal, “Moving obstacle avoidance for large, high-speed autonomous ground vehicles,” in 2017 American Control Conference (ACC), 2017, pp. 5568–5573.
  * [12] W. Luo, W. Sun, and A. Kapoor, “Multi-robot collision avoidance under uncertainty with probabilistic safety barrier certificates,” in Advances in Neural Information Processing Systems, vol. 33, 2020.
  * [13] W. Zhang, S. Wei, Y. Teng, J. Zhang, X. Wang, and Z. Yan, “Dynamic obstacle avoidance for unmanned underwater vehicles based on an improved velocity obstacle method.” Sensors, vol. 17, no. 12, p. 2742, 2017.
  * [14] S. Liu, M. Watterson, S. Tang, and V. Kumar, “High speed navigation for quadrotors with limited on board sensing,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, pp.1484–1491.
  * [15] H. Chen, P. Lu, and C. Xiao, “Dynamic obstacle avoidance for uavs using a fast trajectory planning approach,” in 2019 IEEE International Conference on Robotics and Biomimetics (ROBIO), 2019, pp. 1459–1464.
  * [16] M. Watterson and V. Kumar, “Safe receding horizon control for aggressive mav flight with limited range sensing,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2015,pp. 3235–3240.
  * [17] H. Oleynikova, Z. Taylor, R. Siegwart, and J. Nieto, “Safe local exploration for replanning in cluttered unknown environments for microaerial vehicles,” in IEEE Robotics and Automation Letters, vol. 3, no. 3, 2018,pp. 1474–1481.
  * [18] T. M. Howard, C. J. Green, A. Kelly, and D. Ferguson, “State space sampling of feasible motions for high-performance mobile robot navigation in complex environments,” in Journal of Field Robotics - Special Issue on Field and Service Robotics archive, vol. 25, no. 6, 2008, pp.325–345.
  * [19] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” in IEEE Robotics and Automation Letters, vol. 4, no. 4, 2019, pp. 3529–3536.
  * [20] E.-H. Guechi, J. Lauber, and M. Dambrine, “On-line moving-obstacle avoidance using piecewise bezier curves with unknown obstacle trajectory,” in 2008 16th Mediterranean Conference on Control and Automation, 2008, pp. 505–510.
  * [21] J. Bialkowski, M. Otte, S. Karaman, and E. Frazzoli, “Efficient collision checking in sampling-based motion planning via safety certificates,” The International Journal of Robotics Research, vol. 35, no. 7, pp. 767–796,2016.
  * [22] M. W. Mueller, M. Hehn, and R. DAndrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” IEEE Transactions on Robotics, vol. 31, no. 6, pp. 1294–1310, 2015.
  * [23] M. Ester, H.-P. Kriegel, J. Sander, and X. Xu, “A density-based algorithm for discovering clusters in large spatial databases with noise,” in Proc. 1996 Int. Conf. Knowledg Discovery and Data Mining (KDD’96), 1996, pp. 226–231.