Real-time Identification and Simultaneous Avoidance of Static and Dynamic Obstacles on Point Cloud for UAVs Navigation¹¹1This project is supported by the seed funding for strategic interdisciplinary research scheme of the University of Hong Kong

The raw point cloud is first filtered to remove the noise and converted into earth coordinate $E\!-\!X\!Y\!Z$. The details about the filter are in section 5. We use $Pcl_{t_{1}}$ and $Pcl_{t_{2}}$ to denote two point cloud frames from the sensor at the former timestamp $t_{1}$ and the latest timestamp $t_{2}$ respectively. $t_{2}-t_{1}=d_{t}$ and $d_{t}>0$. The time interval $d_{t}$ is set to be able to make the displacement of the dynamic obstacles obvious enough to be observed, while maintaining an acceptable delay to output the estimation results. $Pcl_{t_{1}}$ and $Pcl_{t_{2}}$ are updated continuously while the sensor is operating. To deal with the movement of the camera between $t_{1}$ and $t_{2}$, $Pcl_{t_{2}}$ is filtered, only keeping the points in the overlapped area of the camera’s FOVs at $t_{1}$ and $t_{2}$ [4]. 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. $Pcl_{t_{1}}$ and $Pcl_{t_{2}}$ are clustered into individual objects using density-based spatial clustering of applications with noise (DBSCAN) [24], resulting in two sets of clusters $OB_{1}=\{ob_{11},ob_{12},...\}$ and $OB_{2}=\{ob_{21},ob_{22},...\}$. Then, matching the two clusters $ob_{2k}\in OB_{2}$ and $ob_{1j}\in OB_{1}$ corresponding to the same obstacle is necessary before the dynamic obstacle identification.

At the time when we obtain the first frame of $Pcl_{t_{2}}$, a list of Kalman filters with the constant velocity model is initialized for each cluster (obstacle) in $OB_{2}$. The position and velocity are updated after the obstacle is matched and the observation values are obtained. To match the obstacle, we preliminarily sort out the two clusters that satisfies the condition $\|pos(ob_{2k})-pos(ob_{1j})\|_{2}<d_{m}$. $d_{m}$ is the distance threshold. $pos()\in\mathbb{R}^{3}$ gives the obstacle position when the input cluster is from the latest frame $Pcl_{t_{2}}$, while it returns the predicted position at $t_{2}$ by the corresponding Kalman filter of the cluster from $Pcl_{t_{1}}$ [4]. It is to associate current clusters to the forward propagated Kalman filters rather than clusters in the previous frame. If we cannot find an $ob_{1j}$ for $ob_{2k}$ there, we skip it and turn to the next cluster ($k\leftarrow k+1$, $k$ is the cluster index). For each Kalman filter, it is also necessary to assign a reasonable maximal propagating time $t_{kf}$ before being matched with a new observation. Because the camera FOV is narrow, we hope to predict the clusters which just move out of the FOV for safety consideration, and they are assumed to continue to move at their latest updated velocity in a short period. A Kalman filter is deleted together with its tracking history if it has not been matched for over $t_{kf}$.

However, matching obstacles with position only may fail when obstacles are getting close, as shown in Figure 3. To improve the matching robustness, we design the feature vector composed of several statistic characters of a point cluster with aligned color information from the obstacle, which is defined as

where $ob$ denotes any point cluster. $len()$ is the function that returns the size of the input cluster. $V^{A}_{P}()\in\mathbb{R}^{3}$ returns the position variance of the cluster, and $V()$ returns the volume of the axis-aligned bounding box (AABB) of the cluster. $M_{C}()\in\mathbb{R}^{3}$ and $V^{A}_{C}()\in\mathbb{R}^{3}$ return the mean and variance of the RGB value of points. The idea is: if there is not a significant difference in the shape and color of the two point clusters extracted from two timely close point cloud frames respectively, then they are commonly believed to be the same object. The global features for each obstacle are very cheap to calculate and proved to be effective in tests.

At last, the Euclidean distance $d_{fte}=\|fte(ob_{1}j)-fte(ob_{2k})\|_{2}$ between feature vectors is calculated with each cluster pair $\{ob_{2k},ob_{1j}\}$ that satisfies the position threshold. For each cluster $ob_{2k}\in OB_{2}$, the cluster $ob_{1J}\in OB_{1}$ results in the minimal $d_{fte}$ is matched with it. The feature vector is normalized to 0-1 since the order of magnitude of each element varies. We use $ob^{m}_{2}\in OB_{2}$ and $ob^{m}_{1}\in OB_{1}$ to represent any two successfully matched clusters.

After the obstacles in two sensor frames are matched in pairs, the velocity of the obstacle $ob^{m}_{2}$ can be calculated by $v^{m}_{2}=\overrightarrow{p^{m}_{2}p^{m}_{1}}/{(t_{2}-t_{1})}$, where $p^{m}_{2}$ and $p^{m}_{1}$ are the position of the corresponding obstacle. In the related works, the mean of the points in each cluster is adopted as the obstacle position, since most of the obstacles in the UAVs application scenarios can be regarded as mass-uniformly distributed. However, due to the self-occlusion, the backside of the obstacle is invisible, so the mean of points is closer to the camera than the obstacle centroid. In addition, the occluded part of a moving obstacle changes when the relative movement occurs between the camera and obstacle. Thus, the relative position between the position mean and the real mass center also changes. This will lead to a wrongly estimated velocity, as shown in Figure 4, the error is mainly distributed on the $Z$ axis of the camera $Z_{cam}$ and it is not a fixed error can be estimated. For the position estimation of obstacles, the self-occlusion is not important, considering the visible part only is safe for obstacle avoidance. But the velocity is the key information of moving obstacles in the vehicle velocity planning. Therefore, we introduce a method to reduce this velocity estimation error by choosing the appropriate track point for the matched pair of clusters, as illustrated in the right figure of Figure 4. For a moving obstacle in translational motion, the closest part to the camera is believed not to be self-occluded in $ob^{m}_{2}$ and $ob^{m}_{1}$. In addition, the middle part of the cluster is close to the centroid of the common obstacles, so the rotational movement is weak in this part. Thus, we only use the center point $\hat{p}^{m}_{2}$ and $\hat{p}^{m}_{1}$ of the closest $N_{c}$ points in the middle part of the cluster $ob^{m}_{2}$ and $ob^{m}_{1}$ to estimate the displacement. $\hat{p}^{m}_{2}$ and $\hat{p}^{m}_{1}$ are named as the track point. Here the distance to the camera is measured only along $Z_{cam}$. The middle part of the cluster is divided in the projection plane corresponding to the depth image. The bounding box for the middle part is shrunk from the AABB of the obstacle to the center proportionally. The shrinking scale factor is $\alpha\ (\alpha<1)$. Considering the common obstacles usually performs slow rotation, and the time gap $d_{t}$ is small, we can neglect the influence to the closest part caused by rotation in the displacement estimation. To update the Kalman filter, $p^{m}_{2}$ (the mean of current cluster) is still the observed position, but the velocity observation is $\hat{v}^{m}_{2}=\overrightarrow{\hat{p}^{m}_{1}\hat{p}^{m}_{2}}/{(t_{2}-t_{1})}$. The classification for static and dynamic obstacle is done by comparing the velocity magnitude with a pre-assigned threshold $v_{dy}$, i.e. $\|\hat{v}^{m}_{2}\|_{2}>v_{dy}$ indicates a moving obstacle. If an obstacle is classified as static in $S_{c}$ consecutive times, the corresponding Kalman filter is abandoned and the static point cluster is forwarded for map fusion.

The Kalman filter for one cluster is detailly described with

where $R,\ H,\ Q$ are the observation noise covariance matrix, observation matrix, and error matrix respectively. The superscript ^- indicates a matrix is before being updated by the Kalman gain matrix $K_{t}$, applicable for the state matrix $\hat{x}_{t}$ and the posterior error covariance matrix $P_{t}$. The subscripts $t$ and ${t\!-\!1}$ distinguish the current and the former step of the Kalman filter. $F_{t}$ is the state transition matrix and $B_{t}$ is the control matrix. $x_{t}$ is composed of the observation of the obstacle position and velocity, 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. $\hat{x}_{t}^{-}$ is also utilized as the propagated cluster state in the obstacle matching introduced above. $\Delta t$ is the time interval between each run of the Kalman filter. In the current stage, we assume the moving obstacle performs uniform motion between $t_{1}$ and $t_{2}$, $a_{t-1}=0$.

We summarize our proposed dynamic environment perception method in Algorithm 1.

To improve the estimation accuracy, we notice the time gap between the latest point cloud message from the camera and the vehicle state message from the IMU in the flight controller. A constant acceleration motion model is adopted to describe the vehicle here, and the ego-motion compensation can be done with

to result in the compensated camera pose $\hat{p}_{cam}$. $p_{cam}\in\mathbb{R}^{2*3}$, $v_{cam}\in\mathbb{R}^{2*3}$ and $a_{cam}\in\mathbb{R}^{2*3}$ are the pose, velocity and acceleration of the camera obtained from raw data (translational and rotational motion), translated from the installation matrix of the camera. $t_{gap}$ is the time gap between the message, equals to the timestamp of point cloud minus the timestamp of vehicle state. As a result, the point cloud can be converted to $E\!-\!X\!Y\!Z$ more precisely.

For non-rigid moving obstacles, for example, walking animals (including humans), the body posture is continuously changing. The point cloud deformation may cause additional position and velocity estimation error of obstacle since the waving limbs of a walking human interfere with the current estimation measurements. We notice that when two neighbor frames of point cloud are overlaid, the point cloud of the human trunk is denser than the other parts which rotate over the trunk. Then, an appropriate point density threshold of DBSCAN can remove the points corresponding to the limbs. So the overlapped point cloud can replace the filtered raw point cloud. For instance, the $w^{th}$ point cloud frame $Pcl_{w}$ is replaced with $Pcl_{w}\cup Pcl_{w-1}$. $\hat{p}_{cam}$ is also replaced by the mean of the value from its neighbor data frame, to align with the point cloud.

The planner receives the moving clusters and the velocity from the dynamic perception module. The remaining clusters are also conveyed to the planner and treated as static obstacles. In addition, we adopt the mapping kit to offer the static environmental information out of the current FOV, because the FOV of a single camera is narrow. To tackle the autonomous navigation tasks, the drone is required to reach the goal position. The desired velocity of the drone is initialized as $v^{\prime}_{des}$, $\|v^{\prime}_{des}\|_{2}=v_{max}$ and $v^{\prime}_{des}$ heads towards the goal. $v_{max}$ is the maximum speed constraints. Also, considering the path optimality, a path planner is usually adopted in the navigation. Thus, the waypoint $w_{p}$ generated from the planned path is used to replace the navigation goal if a path planner is required. Otherwise, $w_{p}$ denotes the navigation goal. $w_{p}$ can be generated from a guidance law or assigned directly as the first waypoint in the path to enable the drone follow the path. It is a choice to combine the velocity planning method with the path planning algorithms to adapt to navigation applications better. Figure 5 illustrates the collision check by calculating the relative velocity of $v^{\prime}_{des}$ towards the obstacles, and if the check fails the velocity re-planning will be conducted. For dynamic obstacles, the relative velocity equals $v^{\prime}_{des}$ minus the obstacle velocity. For static obstacles, the relative velocity is $v_{n}$ itself. If $v^{\prime}_{des}$ is checked to be safe, the finally desired velocity $v_{des}$ is given by $v^{\prime}_{des}$.

The velocity re-planning method is explained in Figures 6-8, the forbidden area (space) is extended to a pyramid for 3D case, different from the triangle for 2D case. If the relative velocity lies in the corresponding forbidden pyramid, four proposed relative velocity vectors are found by drawing a vertical line to the four sides of the pyramid from the relative velocity vector. They are the samples to be checked later. The vertical line segments stand for the acceleration cost to control the vehicle to reach the proposed velocity. For any cluster, $v_{rel}$ denotes the relative velocity, the five vertices of the forbidden pyramid are

The acceleration cost $a_{ci}$ of the proposed relative velocity vectors $v_{pi}\ (i\in\{1,2,3,4\})$ can be calculated by solving the 3D geometric equations, as follows:

In (12)-(15), triangle $\{\hat{p}_{n},vt_{1},vt_{2}\}$ is taken as the example, $C^{p}[x,y,z]^{T}+c^{p}_{d}=0$ is the corresponding plane equation, $\hat{p}_{n}$ is the common vertex of all the 4 triangles.

Obviously, for only one obstacle, the desired relative velocity with the minimal cost is from the four proposed ones. For multiple obstacles and forbidden pyramids, the desired relative velocity is chosen by comparing the feasibility, reachability and cost. Among all the proposed relative velocity vectors, the one checked to be feasible and reachable and with the minimal cost is selected ($v^{r}_{des}$), and the desired vehicle velocity $v_{des}=v^{r}_{des}+v_{obs}$. $v_{obs}$ is the velocity for the corresponding obstacle. Although the globally optimal solution for acceleration cost cannot be guaranteed within the samples, the computation complexity is greatly reduced comparing to solve the optimal solution. We use the inflated bounding box because the character radius of the vehicle $r_{uav}$ can not be neglected.

The feasibility check is to guarantee $v_{des}$ is safe for all obstacles, not for only one of them, which is described in Figure 7. Besides the feasibility check, $v_{des}$ should satisfy the maximum speed constraints $v_{max}$. We introduce the reachable set for the relative velocity vector to check if the proposed relative velocity $v_{pi}\ (i\in\{1,2,3,4\})$ is reachable, as Figure 8 indicates. For the relative velocity towards one obstacle, $v_{rel}=v_{n}-v_{obs}$ always hold. $v_{rel}$ is the current relative velocity towards the obstacle, $v_{obs}$ is the obstacle velocity.

In addition, the lag error of the velocity planning caused by the time cost to reach the desired velocity is also considerable. The relative displacement between the moving obstacle and vehicle during this time gap should be estimated, because the forbidden pyramid is also directly related with the relative position. We can assume the solved jerk $J_{n}$ is very close to its boundary $J_{max}$, because the time cost $t_{v}$ is minimized in the optimization problem (20). Thus, the time cost is estimated as

and the displacement of the vehicle and obstacle is calculated by

At last, the estimated displacement $d_{uav}$ and $d_{obs}$ are added to the position after $v_{des}$ is obtained, and a new $v_{des}$ is planned in iteration until it is checked to be safe. The accurate time cost $t_{v}$ can only be determined after solving the motion optimization problem. However, involving the optimization problem in the iteration will be time-consuming, so we use a close-form solution as the approximate value. To speed up the convergence, the safety radius is inflated by a small value $\epsilon$ (equivalent to the tolerance in the safety check) to calculate $v_{pi}$:

For a situation where the obstacles are too dense, the forbidden area may cover all the space around the vehicle. We first sort all the clusters with the increasing order of distance to $p_{n}$, the farther obstacles are considered less threatening for the drone. Then, the $j^{th}$ and the next clusters are excluded, $j$ is the iteration number increasing from 0. Algorithm 2 reveals the process of velocity planning. As a result, the vehicle can always quickly plan the velocity to avoid static and dynamic obstacles and following the path to meet the different task requirements.

After the desired velocity $v_{des}$ is obtained, it appears as the constraint in the motion planning and will be reached in a short time. The waypoint constraints $w_{p}$ is also considered to follow the path, as shown in Figure 9.

The optimization problem to obtain motion primitives is constructed as

where the jerk of the vehicle $J_{n}$ is the variable to be optimized. $t_{v}$ is the time required to reach $v_{des}$, which is the variable and the optimization object at the same time. $a_{n+1}$ and $p_{end}$ are calculated by the kinematic formula. $a_{max}$ and $J_{max}$ are the kinodynamic constraints of acceleration and jerk of the vehicle respectively. The velocity constraint $v_{max}$ is satisfied in the equality constraint with $v_{des}$. $\eta_{1},\ \eta_{2}$ are coefficients. The default values are shown in Table 1. After the desired trajectory piece is solved, a default cascade PID controller of PX4 is utilized to track this trajectory in position, velocity, and acceleration.

For the dynamic environment perception, filtering the raw point cloud is necessary, because the obstacle state 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 as our former work [2], as shown in Figure 10, but the parameters are different. Distance filter removes the points too far ($\geq 8\ m$) from the camera, voxel filter keeps only one point in one fixed-size ($0.1\ m$) voxel, outlier filter removes the point that does not have enough neighbors ($\leq$ 13) in a certain radius ($0.25\ m$). Based on such configuration, the density threshold for DBSCAN is at least 18 points in the radius of $0.3\ m$. These metrics are tuned manually during extensive tests on the hardware platform introduced in the next subsection, 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 pose estimation of the camera (including the ego-motion compensation) is not accurate enough for dynamic obstacle perception. To solve this problem, we propose a practical and effective measurement: The filtered point cloud is accepted only when the angular velocity of the three Euler angles of the drone is within the limit $\omega_{max}$.

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 1 shows the parameter settings for the tests. The supplementary video for the tests has been uploaded online²²2https://youtu.be/1g9vHfoycs0.

We set up a hardware test environment as Figure 15(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 Figure 15(c), 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 solid line shows the trajectory of the drone while the blue dashed 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 Figure 16, the velocity curve indicates that the drone reacts very fast once the moving obstacle appears. In Figure 16(b), the blue line is only for the path planning algorithm (HAS), and the red line represents the whole planning with 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. The red and yellow line show the irregularity, which is 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. The single-step time cost of our proposed method (exclude the path planning) is even smaller than 0.01 s, indicating the fast-reacting ability towards moving obstacles.

At last, we compare our work with SOTA works on the system level in Table 4. Since most related works differ significantly from ours in terms of application background and test platform, for numeral indicators we only compare the total time cost for reference. The abbreviations stand for: obs (obstacle), cam (camera), UUV (underwater unmanned vehicle), N/A (not applicable). “N/A” in the sensor type column refers to the work that gets obstacle information from an external source and does not include environment perception. Most works have severe restrictions on the obstacle type or incompleteness in environment perception, and the computing time cost is not satisfactory for real-time applications. Our work has a great advantage in generality and system completeness, the time cost is also the shortest unless the event camera is considered.