Autonomous Quadrotor Navigation With Vision Based Obstacle Avoidance and Path Planning

Due to the high demands on military and commercial applications, the development of UAVs (unmanned aerial vehicles) has become increasingly important in recent years. In this paper, we present a vision guided autonomous navigation approach for quadrotor UAVs. A map-based offline path planning technique is developed to generate an initial path, followed by the waypoints of the trajectory for flight guidance. During the navigation, an onboard camera is utilized to acquire a sequence of monocular images for environment perception. A vision-based obstacle detection technique using optical flow is proposed for collision avoidance. The optical flow field constructed from the image sequence is used to provide the depth cues for the incoming obstacle detection. A single-board computer is adopted as a control platform, and the proposed algorithms are implemented for online and real-time processing. Several experiments are carried out in the outdoor environment for obstacles avoidance and visual guidance. The results have demonstrated the feasibility of our proposed method for path planning and autonomous navigation.


I. INTRODUCTION
In recent years, the development and applications of UAVs (unmanned aerial vehicles) has become more and more popular. While UAVs were mainly used for the military and civil operations such as aerial inspection and terrain modeling in the past few decades, it is now growing rapidly for commercial uses [1], [2]. Due to the advances of communication, control and computing technologies, as well as the hardware cost-down, the industry has put significant efforts on making the UAV an indispensable tool in our daily life. Many companies start to provide some commercial services such as air delivery, aerial imaging [3], etc. In this IoT era, people are also able to connect to UAVs via smartphones for various routine jobs or innovative applications. By the integration with IMU (inertial measurement unit), GPS and other sensors, the realization of autonomous flight is becoming possible, but still requires further investigation [4].
The associate editor coordinating the review of this manuscript and approving it for publication was Emre Koyuncu .
According to the design principle, there are two types of UAVs commonly available in the market: fixed-wing drones and quadcopters. The fixed-wing drones are able to provide long endurance flight, but the motion control is not as flexible as quadcopters. They require a much larger radius of gyration for direction change, and are generally used for aerial imaging applications. On the other hand, the control of quadrotors is more stable, flexible, and able to provide the vertical takeoff, landing and hover [5]. However, the payload and flight endurance are very limited. Nevertheless, operating remotely from the ground station would not fulfill the requirements for most recent applications. The path planning, navigation and safety issues of autonomous flight are widely discussed in the robotics community [6].
In the robotics research areas, autonomous navigation generally includes the capabilities of path planning and obstacle avoidance. One important objective is to provide an optimal path for safe navigation. For the UAV applications, an initial trajectory is usually provided for navigation guidance. The waypoints from path planning are then used to instruct the heading control of the UAV [7]. In general, the flight trajectories are planned off-line, with the assistance from the geographic information system (GIS). Similar to the map-based vehicle or mobile robot navigation, UAV flight planning will generate a shortest or optimal path in the virtual environment. Different from the ground vehicles, it is possible to avoid the obstacles by creating a high altitude flight path. Nevertheless, people are more concerned with the quadrotor UAVs flying in the low altitude environment. Thus, the related safety issues need to be carefully addressed.
This paper investigates autonomous navigation of quadrotor UAVs in the real-world environment. While the quadrotors have been adopted for many applications over the years, the ability to keep the flight safe in autonomous navigation is still a great challenge [8], [9]. Due to the limited payload capabilities and low cruising endurance, it is required to use lightweight onboard devices for the UAVs [10]. Consequently, the sensors such as LIDAR or sonar are typically too large, heavy or expensive to install on quadrotors [11]. In this work, we utilize a low-cost camera system with vision-based algorithms for the obstacle avoidance. With map-based path planning prior to the autonomous navigation, the main objective is to avoid collision during the flight. This is specifically important in a low altitude flight with the obstacles not shown on the maps, such as trees, traffic lights, and moving objects [12]. More detailed information and literature review about collision detection for low-altitude flight can be found in [13].
In the existing literature, vision-based obstacle avoidance approaches for the UAV can be divided into three categories [14]. The first approach is based on the monocular visual cues. It relies on the characteristics of acquired image sequences to provide the contour or texture information of the objects [15]. The regions in the image can then be classified into obstacles or non-obstacles. In recent work, Yang et al. [16] proposed a lightweight probabilistic CNN (convolutional neural network ) for monocular depth prediction and obstacle avoidance. The technique was integrated with a visual odometry method to improve the prediction accuracy. Their algorithm was able to run in real-time on a GPU platform. He et al. [17] presented a bio-inspired monocular vision perception method for obstacle avoidance of micro UAVs. Since the accurate distances were not calculate explicitly, the technique was more computational efficient. They provided mapless navigation using a local planner is trained by deep reinforcement learning.
For the stereo vision based obstacle avoidance approach, multiple cameras are used for stereo matching and depth computation [18]. In the previous work, Park and Kim proposed a collision avoidance algorithm using a stereo sensor [19]. Except for the derivation of depth information, the concept of the collision cone by considering the flight velocity was adopted for collision avoidance from the quadrotor's point of view. Oleynikova et al. [20] presented a lightweight on-board stereo vision system using FPGA. It was designed for MAV (micro aerial vehicles) applications, and able to run up to 60 frames per second. Alternatively, a stereo vision based avoidance strategy which allows constant speed maneuvers was proposed [21]. The approach was able to deal with nonholonomic motion constraints, and implemented on a flapping wing MAV. In earlier work, a binocular stereo method was adopted for obstacle detection [22]. It was used to develop a visual threat awareness system for the UAV's real time collision avoidance.
The third approach for obstacle avoidance is to use the motion parallax associated with the camera motion. A common implementation is the optical flow method which utilizes the motion flow obtained from a sequence of images. It can be combined with either single or multiple cameras to detect the objects or environment structures in the scene. In [23], Cho et al. presented an optical flow technique specifically for textured 3-D environment. The divergence at the focus of expansion was considered to deal with the wall-like frontal obstacles. McGuire et al. first used stereo vision to estimate the depth information of the environment [24]. The results were then combined with optical flow for the velocity computation and obstacle avoidance. There were also some works which considered the obstacle detection simultaneously for multiple quadcopters [25], [26]. Nevertheless, these techniques did not take the flight planning into account to improve the real-time computation for collision detection.
In this paper, we integrate both the online obstacle avoidance and offline path planning scheme for autonomous flight control. A map-based offline guidance technique is adopted to generate an initial path using the rapidly-exploring random tree (RRT) algorithm. It is then followed by the waypoints of the generated path for flight guidance. During the navigation, an onboard camera is utilized to acquire a sequence of images for the optical flow field construction and obstacle detection as illustrated in Figure 1. It provides the relationship between the quadrotor motion and the obstacles for navigation. Several machine vision techniques are adopted to increase the robustness of our approach. The proposed algorithms are finally implemented on a single-board computer for online and realtime processing. We have performed experiments with various environment conditions. The results have demonstrated that the proposed method is able to avoid objects and reroute to the destination.
The rest of the paper is organized as follows. First, we describe the obstacle avoidance algorithm in Section II. The description of the autonomous flight system is then provided in Section III. To evaluate the proposed method, a number of experiments are conducted, and the results are presented in Section IV. The conclusion is finally drawn in Section V.

II. OBSTACLE AVOIDANCE SYSTEM
A system flow of the proposed obstacle avoidance technique is illustrated in Figure 2. We develop the vision-based algorithms for real-time image processing and analysis. It consists of several stages, starting from the image acquisition, preprocessing, optical flow computation, obstacle detection, and the final decision making. In general, the optical flow based  approaches are difficult to achieve the real-time performance on the low-cost hardware. Thus, one of the main objectives of the proposed technique is to reduce the computational cost in various situations.
In the pre-processing stage, the acquired image sequence is first down-sampled to reduce the computational load for the subsequent processing steps. The images are then converted to grayscale, and Gaussian filtering is carried out for noise removal. Furthermore, only the central region of the image needs to be considered for obstacle avoidance if the camera's field-of-view (FOV) is sufficiently large. It should be noted that when the quadrotor UAV proceeds forward, there will be a tilt angle facing downward for the pose. Thus, the image captured during the quadrotor motion as shown in Figure 3(b) is different from the image obtained statically (as shown in Figure 3(a)). Consequently, the region-of-interest (RoI) in the implementation is given by the red bounding box as shown in Figure 5(b) for obstacle avoidance. The overall computation can then be further reduced.
To detect the changes in an image, the optical flow methods are commonly used techniques. The optical flow in the image is used to identify the motion vectors induced by the relative motion between the camera and the environment. In this work, the dense optical flow technique is adopted to calculate the dynamic change in the scene [27]. Compared to sparse optical flow methods [28], it is able to calculate the displacement of each pixel between two consecutive image frames extensively. Our optical flow estimation is developed based on Farnebäck's approach [29]. It consists of the assumptions of brightness constancy and smoothness constraint [30]. This method approximates the displacement of neighboring pixels using a quadratic polynomial expansion, and provides better results in terms of computation speed and accuracy. However, there are still restrictions for its use on the obstacle avoidance algorithm. Some issues include the optical flow vanished due to the slow camera movement, sensitive to the leaf trembling, illumination change and background noise in outdoor scenes. Thus, it is necessary to combine with other techniques for the motion analysis of potential obstacles.
In the optical flow method [31], the images are acquired from the camera continuously, and can be treated as a function of time: I (t). The brightness of a pixel (x, y) at time t can be expressed by I (x, y, t). That is, the image is treated as a function of time and position. Its brightness range is the intensities of the pixels in the image. If we consider a 3D scene point with the corresponding image point at (x, y), the image coordinates will change when the camera is in motion. The projection of the 3D scene point in the image is calculated at other time instants, while the pixel brightness value of the same spatial point is fixed among the image sequence. Given a pixel at the position (x, y) at time t, we assume that it moves to the position (x + dx, y + dy)dt at time t + dt. Since the brightness remains the same, we will have In the above equation, the constant brightness assumption is strong and not always satisfied in the real situations. The image intensity might be different due to the surface material of the object. Moreover, the camera exposure adjusted with automatic settings will make the image darker or brighter. Nevertheless, it still provides sufficient visual cues for the outdoor environment with ambient light.
Applying Taylor expansion, the optical flow equation (1) can be approximated by Under the constant brightness assumption, the intensity value remains unchanged over time, so we have ∂I ∂x where dx/dt and dy/dt are the displacements of a pixel in the x and y directions per unit time and denoted by u and v, respectively. Similarly, ∂I /∂x and ∂I /∂y are the image gradients in the x and y directions and denoted by I x and I y , respectively. Let I t be the brightness change of the image with respect to time. Then, Eq. (3) can be written in the matrix form To derive the pixel displacement (u, v), it is further assumed that the pixels in a certain window have the same motion in the optical flow. Thus, taking an image region with a w × w window size, there will be totally w 2 equations: . . .
Then the equation can be rewritten as This overdetermined linear system can be solved by minimizing the least squared error, i.e., Thus, the displacement (u, v) of a pixel can then be calculated accordingly.
The information derived from the optical flow field is the relationship between velocity and depth, not the absolute distance. When a camera is moving forward, the closer regions have larger motion vectors. On the other hand, the smaller motion vectors are associated with the further regions. This fact can be applied to the optical flow for the detection of nearby objects from the image. If the magnitudes of the optical flow vectors are less than a pre-set threshold in an image region, there will be no obstacles along the line-ofsight direction. Although these properties are satisfied when a quadrotor proceeds forward, the optical flow field could be unstable if the motion direction is changed, for example, at the navigation waypoints. In these cases, a false obstacle alarm might be reported. To deal with this problem, one way is to ignore or compensate the optical flow vectors induced by unexpected camera motion or shaking. Since the optical flow field represents different motion directions caused by the movements of the camera and objects in the scene, we can extract the independent motion regions associated with the objects by image segmentation. To verify if a region belongs to an obstacle from the optical flow field, a series of process is conducted.
First, if the motion vectors are larger than a given threshold for over 70% of the optical flow field, it is considered as the result due to the camera shaking and the current image frame is discarded. The onboard IMU data are then used to check the state of the quadrotor. If the lateral motion is instructed by a control command, the optical flow computation is compensated using the IMU data. As shown in Figure 4, there is severe optical flow in the lateral direction. After the motion compensation, only three regions near the corners are preserved, which are associated with the new image content acquired from the scene.
The obstacle detection using the optical flow field is illustrated in Figure 5. Given an input image sequence as shown in Figure 5(a), the optical flow computation result is illustrated in Figure 5(b). The motion vectors marked in blue represent the optical flow with the magnitude larger than a threshold, and the segmentation result is shown in Figure 5(c). In most cases, the obstacles in the foreground will be still mixed with the background. To deal with this problem, we utilize the property that the motion vectors in the image which are associated with the same distance in the scene should possess similar magnitudes. Consequently, the noisy optical flow with uncertainty can be filtered by a clustering process [32]. In the implementation, a sliding window is adopted to combine the motion vectors with comparable magnitudes by the following steps: • A 5 × 5 sliding window is used to merge the motion vectors with similar magnitudes.  • Compare the motion vector not belonging to any clusters with the clustered vector.
• Create a new cluster if the motion vector does not belong to the existing ones.
• Repeat until all motion vectors are clustered. By removing the outlier motion vectors with extremely small or large values, the foreground and background are shown in Figures 5(d) and 5(e), respectively. It should be noted that the optical flow computation is used for online analysis in the real scene. There is no ground truth data for evaluation in the implementation.
To make the obstacle avoidance algorithm more robust in terms of definite threats under various situations, two safety conditions are included in the system development as illustrated in Figure 6. First, if the object area is not sufficiently large in the RoI (more than 50% in the experiments), it is assumed the obstacle is beyond the restricted range and the quadrotor proceeds as planned. This setting is to ensure that the quadrotor detours when the obstacle is sufficiently close (1) guidance system, (2) navigation system, and (3) control system. and may have a potential threat. Second, the motion direction of the flight will be changed only if the obstacles are detected in several consecutive image frames. This criterion is adopted to avoid the false alarms by temporal filtering. As for the strategy for obstacle avoidance, the quadrotor will turn to the opposite side of the obstacle until it is out of the RoI. These include the left and right turns, and the lift operation. More advanced maneuver might be investigated by consider the power consumption and other motion control issues.

III. AUTONOMOUS NAVIGATION SYSTEM
The obstacle avoidance algorithm presented in the previous section is integrated with the path planning technique to build an autonomous navigation system. As illustrated in Figure 7, it consists of three subsystems for the autonomous flight: (1) guidance system, (2) navigation system, and (3) control system [33]. The path planning is used for flight guidance, autonomous navigation, and motion control. It can be divided into two parts, the static and dynamic planning. The objective of static planning is to derive an optimal navigation path in advance based on the map information. During the autonomous navigation, it is required to adjust the planned motion path if obstacles are encountered. The dynamic planning is to integrate the obstacle avoidance algorithm and generate a new path to the destination.
The static path planning is carried out in the offline environment, with the consideration of terrain restriction and quadrotor limitation. It provides the destination, waypoints, and other navigation constraints such as the flight altitude. If there are no obstacles in the environment, the ideal planned path is a straight line between two waypoints. The objective is to provide a safe trajectory for quadrotor navigation. To fulfill this purpose, the large objects such as buildings and man-made structures are modeled in the virtual environment for path planning. In the implementation, we use the satellite images obtained from Google Earth for 3D modeling of the environment, as illustrated in Figure 8 (the generated 3D model is shown in Figure 13). The GPS coordinates are used for the building setting. The RRT algorithm is developed for path planning with the knowledge of the working space [34]. The generated 3D waypoints are then transferred to GPS coordinates for the real scene navigation.
In the early research, Kavraki et al. proposed a probabilistic roadmap approach for path planning [35]. They presented a two-phase method to deal with the robot path planning problem in static workspaces. The rapidly-exploring random tree (RRT) algorithm is first presented by LaValle et al. [36], and followed by further modifications and applications [37], [38]. It is originally designed for a broad class of path planning problems, and is suitable for complex and high dimensional spaces. However, the motion cost is not explicitly considered in the traditional RRT approach. In this work, we use the RRT* algorithm for optimal path planning [39]. It is a sampling-based planning algorithm which is able to quickly find an initial path and perform an optimization for the path. We first use the map from Google Earth to mark the obstacle location, and then build the three-dimensional environment. The pseudo code of our RRT* implementation is described in Algorithm 1.  The RRT* algorithm is extended from the traditional RRT algorithm with an additional cost function. It takes the neighborhood of a new point into consideration to compute the cost of the entire path. As illustrated in Figure 9, the path is updated at each iteration according to the cost of the path from Z init to Z new . Thus, the path planning process output is a series of waypoints for quadrotor navigation. When the quadrotor reaches a waypoint, the next waypoint will be used to guide it one step towards the destination. In the pseudo codes, RRT* starts from the root of the tree. For each iteration, the tree is built with random sample points from the space. The nearest routine will find the node in the tree nearest to the new random sample point. The steering routine generates a new point reachable to the random point. It is then verified if the nearest point satisfies the environment constraint. The new point will be added to the tree if its path to the nearest point is collision free. If the distance between the current position and the goal is smaller than a threshold, the procedure is terminated and an initial path is derived. RRT* will continue updating the path while the number of the iterations increases. In the experiments, it is implemented for off-line processing to provide sufficient iterations. The path planning result is then integrated with the 3D simulation environment constructed according to the real scenes using OpenGL.

Algorithm 1 Probabilistically
During the flight navigation, the quadrotor follows the path provided by the initial planning. When the quadrotor moves forward, the navigation system will use the onboard camera to detect the approaching obstacles by the optical flow based avoidance technique described in the previous section. If the quadrotor detours from the initial path according to the commands sent by the obstacle avoidance system, the guidance system will generate a new path based on the quadrotor position and the next waypoint. For the autonomous flight system, we propose two control strategies, namely following the planned path and the obstacle avoidance scheme. As shown in Figure 10, the control system receives the commands from the obstacle avoidance system, and transmit the PWM signals to each motor to adjust the pose of the quadrotor. Based on the optical flow field, the quadrotor will be instructed to turn away from the large motion vectors.

IV. EXPERIMENTS AND EVALUATION
To evaluate the proposed vision guided quadrotor navigation techniques, several experiments are conducted with low altitude flight. The autonomous navigation is carried out in a few outdoor environments containing obstacles. Figure 11 shows the UAV system used in our experiments. It is developed based on a commercially available quadrotor, Parrot AR. Drone, with hardware and software modifications. The specification of the quadrotor is tabulated in Table 1. An embedded single-board computer attached on the quadrotor is used as our control platform. It is an ARM based system running on the Linux OS. The image sequences are acquired from the built-in camera of the quadrotor, and transmitted to the onboard embedded system via the Wi-Fi communication for processing. The control commands are then sent back to the quadrotor.
In our experiments, the image processing time for a frame on the embedded platform is about 150-180 ms, which corresponds to about 6 fps (frames per second). The optical flow and other image processing algorithms are implemented based on the OpenCV library with slight modification and parameter settings for the application scenarios. In the experiments, the normal UAV flying speed is about 1 m/s. There will be about 6 frames under processing for 1 meter fly. The UAV  will slow down when it is close to an obstacle for direction change and avoidance. Consequently, the UAV flying speed does not affect the image processing computation in a noticeable way.
Before the quadrotor's navigation in the outdoor environment, the path planning is carried out first to provide a safe route. The RRT* algorithm is used to generate an initial path. Figure 12 shows the RRT* simulation results with the cost of the entire navigation path. The optimal path is updated based on the generated random tree for each iteration. In the experiment, the numbers of iterations are set as 500 and 800 for comparison. The random tree spans wider when the number of iterations increases, and it is more likely to obtain the optimal path. Figure 13 shows the RRT* path planning result visualized using OpenGL for an outdoor environment. The two workable paths marked in red are both near optimal and collision-free. If there is no restriction on the flight altitude, it is always possible to raise the quadrotor high enough and proceed in a straight line. Nevertheless, we are more interested in the autonomous flight in low altitude environments. In the first experiment, the quadrotor is moving towards an iron stair as shown in Figure 14(a). Several captured images and the optical flow field are shown in Figure 14(b). It is to verify if our method can detect and avoid the obstacle in the waypoint direction. When the quadrotor moves along the planned path, the system will check whether it satisfies the warning condition. If the criteria are met, the flight direction  will be adjusted by the control system for collision avoidance. Figure 15 shows the trajectory bypassing the iron stair. The guidance system will redirect the path toward the next waypoint after the obstacle is disappeared in the image.
We have also performed the obstacle avoidance for various situations, including fence, trees, and street light. The results for several tests and success rates are tabulated in Table 2. It shows that our algorithm performs well in the single obstacle cases such as the tree or iron stair. The low success rate for VOLUME 9, 2021  multiple obstacles is mainly due to the optical flow field not able to separate the objects and the background very clearly.
Finally, the proposed vision guided navigation with path planning is tested in a large scale environment. Figure 16 shows the experiment carried out in a football field of about 200 × 200 square meters containing a large building. The planned path is shown in orange lines on the Google map, and the waypoints are indicated by the yellow thumbtacks. The blue lines in the figure show the trajectory of the actual flight. It consists of several flight direction changes due to the encountered obstacles (tree, stair and building) as shown in Figure 17. In addition, the quadrotor makes turns earlier as planned at the thumbtack location. This is due to a preset safety distance of 5 meters to compensate the limited GPS accuracy.

V. CONCLUSION
In this work, we present a vision guided autonomous flight system for quadrotor navigation. It consists of path planning and real-time obstacles avoidance techniques. Our method can detect the frontal obstacles and guide the quadrotor with a proper direction. The proposed algorithms are developed for an embedded platform to achieve the online processing capability. We have reduced the computational requirement of the optical flow method. The experiments are carried out in the outdoor environment with single or multiple obstacles. The results demonstrate the feasibility of our approach for vision guided quadrotor navigation. In the current system, we do not specifically consider the presence of moving obstacles. More advanced image-based approaches will be investigated in the future work. He is also a Professor and the Director of the Robot Vision Laboratory, National Chung Cheng University. He has authored more than 170 journal articles and conference papers, and has written two book chapters. He also holds 11 U.S. patents and nine Taiwan patents. His research interests include computer vision, robotics, machine learning, and image processing. He is a Senior Member of OSA. He serves as an organizing committee member and a program committee member for more than 50 international conferences. He was a recipient of the Outstanding Robotics Engineer Award from the Robotics Society of Taiwan and the Outstanding Academic-Industry Cooperation Award from the Taiwan Association of System and Science and Engineering.
XIN-ZHONG PENG received the B.S. degree in electronics engineering from Chung Yuan Christian University, and the M.S. degree in electrical engineering from the National Chung Cheng University, Taiwan. He is currently with Taiwan Semiconductor Manufacturing Company, Hsinchu, Taiwan. His research interests include robotics, embedded systems, mechatronics, and computer vision. VOLUME 9, 2021