Vision-Based Obstacle Avoidance Strategies for MAVs Using Optical Flows in 3-D Textured Environments

Due to payload restrictions for micro aerial vehicles (MAVs), vision-based approaches have been widely studied with their light weight characteristics and cost effectiveness. In particular, optical flow-based obstacle avoidance has proven to be one of the most efficient methods in terms of obstacle avoidance capabilities and computational load; however, existing approaches do not consider 3-D complex environments. In addition, most approaches are unable to deal with situations where there are wall-like frontal obstacles. Although some algorithms consider wall-like frontal obstacles, they cause a jitter or unnecessary motion. To address these limitations, this paper proposes a vision-based obstacle avoidance algorithm for MAVs using the optical flow in 3-D textured environments. The image obtained from a monocular camera is first split into two horizontal and vertical half planes. The desired heading direction and climb rate are then determined by comparing the sum of optical flows between half planes horizontally and vertically, respectively, for obstacle avoidance in 3-D environments. Besides, the proposed approach is capable of avoiding wall-like frontal obstacles by considering the divergence of the optical flow at the focus of expansion and navigating to the goal position using a sigmoid weighting function. The performance of the proposed algorithm was validated through numerical simulations and indoor flight experiments in various situations.


Introduction
To navigate through unknown environments, micro aerial vehicles (MAVs) need to detect and avoid obstacles. There are many types of sensors to detect obstacles such as RADAR (RAdio Detection And Ranging), LiDAR (LIght Detection And Ranging), and ultrasonic sensor. RADAR and LiDAR have a good performance in terms of the operation range and accuracy to detect obstacles around the MAV. However, they have disadvantages in terms of operating time due to heavy weight and energy consumption. Although lightweight and low power consumption products have been developed recently [1], they are still expensive. Ultrasonic sensors are small and light but typically have poor range accuracy. Vision sensors, on the other hand, have various advantages such as lightweight, large detection area, fast response time, low cost, and rich information about the environment around the MAV.
There are various obstacle avoidance methods using the vision system. Tomoyuki et al. [2] and Abdulla et al. [3] conducted obstacle avoidance experiment with a monocular camera using the

Optical Flow
The optical flow refers to the movement of each pixel expressed in the image plane when an object moving in the three-dimensional space is projected onto the two-dimensional image plane. There are various methods to estimate the optical flow such as [15][16][17]. Recently, learning-based optical flow estimation methods are also proposed [3,18,19]. In this work, the optical flow computation method proposed by Horn and Schunck [16] is adopted. The concept for computing optical flows is briefly introduced in the following for the sake of the completeness of the paper and readers' convenience.
In the Horn-Schunck method, two assumptions are used to estimate the optical flow. First, as in Figure 1, the intensity of a particular point is constant over time. dI(x o f , y o f , t) dt = 0 where I is the intensity of the pixel at the point (x o f , y o f ). By using the chain rule, Equation (1) can be expressed as: where I x , I y and I t are partial derivatives of the intensity at each pixel with respect to where ∇I = (I x , I y ) T , u and v are x o f and y o f directions of the optical flow, respectively. The cost function to minimize the change of the intensity over the time is defined by: The second assumption is a smoothness constraint, which represents that the neighboring points on an object have a similar optical flow. Having a similar optical flow at the neighboring points implies that the magnitude of the gradient of all optical flows should be minimized. It can be expressed by: The total cost function to be minimized is designed using Equations (4) and (5) by where α is a weighting factor for the smoothness constraint. Then, u and v minimizing Equation (6), which are the optical flow at each pixel, can be obtained: v =v − I y I xū + I yv + I t From Equations (7) and (8), an iterative solution can be obtained as: v k+1 =v k − I y I xū k + I yv k + I t where u k+1 and v k+1 are the optical flow of the x o f and y o f axes at the k + 1th time frame, respectively.

Obstacle Avoidance Strategy
This section introduces the obstacle avoidance strategies using the optical flow estimated from a forward looking monocular camera. The horizontal balance strategy vertical balance strategy to avoid the obstacle vertically, and the frontal obstacle avoidance strategy which exploits the expansion of the optical flow are introduced. In addition to obstacle avoidance strategies, the waypoint guidance strategy is described to reach to the goal point for the MAV. Lastly, the above strategies are integrated with the weighted sum of guidance commands from different strategies according to obstacle environment conditions.

Horizontal Balance Strategy
This subsection deals with the horizontal balance strategy. We modified conventional balance strategies [5,20,21] using the concept of the proportional-derivative (PD) controller. First, we show how the magnitude of the optical flow changes as the MAV approaches to an obstacle, and then the methodology of the horizontal balance strategy is introduced.
As shown in Figure 2, suppose there is the point P on the obstacle of interest corresponding to the point p on the image plane. The vector R is defined as the vector from the nearest point H on the principal axis of the camera to the point P. The vector r is the vector corresponding to R on the image plane. The MAV approaches the obstacle with the forward speed V x at the point O. Note that the focal length f and OH = x are given. Considering the geometrical relationship, the following relation holds: It is worth noting that r and x are functions of time t and f and R does not change as the MAV moves. Differentiating Equation (11) with respect to time gives: By rewriting Equation (12), the optical flowṙ = M r for a particular point of the obstacle is obtained as: where the speed of the MAV V x = −ẋ. In Equation (13), the denominator is a function of time, whereas the numerator is constant with the assumption that the MAV keeps a constant speed. Note that the magnitude of the optical flow M r increases as the MAV approaches the obstacle. Let the image be equivalently split into two horizontal (left and right) and vertical (upper and lower) half planes. Considering the fact that the larger the magnitude of the optical flow is, the closer the obstacle is, the desired heading direction of the MAV can be determined by finding the half plane which has the smaller sum of optical flows. More specifically, the optical flow is calculated within the horizontal calculation window shown as the red area in Figure 3 to ignore the unnecessary part of the environment where collision does not need to be considered. The horizontal optical flow calculation window is the center region among five evenly-divided regions of the image plane horizontally. The size and position of the horizontal optical flow calculation window were empirically determined. The heading rate command (ψ rl d ) is generated by the PD controller with the error (e rl ) defined as the difference of the sum of the optical flows at left (M le f t = ∑ le f t u 2 i + v 2 i ) and right (M right = ∑ right u 2 i + v 2 i ) half planes, as shown in Figure 3:ψ rl d = k P,rl e rl + k D,rlėrl (14) where k P,rl and k D,rl are positive gains for the horizontal balance strategy. Gain values were set to k P,rl = 0.01 and k D,rl = 0.01 in the numerical simulations presented in Section 4, and k P,rl = 0.007 and k D,rl = 0.001 in the indoor flight experiments presented in Section 5.

Vertical Balance Strategy
To avoid the obstacle in the three-dimensional environment, the MAV has to change the altitude as well as heading angle. In this subsection, the obstacle avoidance strategy by changing the altitude of the MAV is proposed. The optical flow for the vertical balance strategy is calculated within the vertical calculation window shown as the red area in Figure 4 where the unnecessary part to avoid the obstacle is ignored. The vertical optical flow calculation window is obtained similarly to the horizontal calculation window by dividing the image plane vertically. The size and position of the vertical optical flow calculation window were empirically determined. The desired climb rate command (ḣ d ) can be generated by the PD controller with the error (e ud = M down − M up ), which is the difference of the sum of optical flows at lower (M down ) and upper (M up ) half planes shown in Figure 4. However, when the MAV moves with a high climb rate, the additional optical flow could be generated due to the effect of vertical movements, and, as a result, the error e ud could diverge unexpectedly. To avoid the unwanted effect fromḣ, the error e ud for the PD controller is modified to: e m,ud = e ud 1 + k P,m |ḣ| (15) where k P,m = 1000 is the weighting for the magnitude of the climb rate. In Equation (15), ifḣ = 0, then, e m,ud is equal to e ud and the larger the climb rate is, the closer the e m,ud is to zero so that the MAV does not change the altitude much. The PD controller for vertical balance strategy can then be defined as: where k P,ud and k D,ud are positive gains of the vertical balance strategy. Gain values were set to k P,ud = 0.01 and k D,ud = 0.01 in the numerical simulations presented in Section 4, and k P,ud = 0.03 and k D,ud = 0.01 in the indoor flight experiments presented in Section 5.

Frontal Obstacle Avoidance Strategy
Horizontal and vertical balance obstacle avoidance strategies might not work when the obstacle is directly in front of the MAV. In this situation, the optical flows on both the half planes are similar, so the MAV would go forward without changing the heading angle and collide with the obstacle. To address this problem, the concept of expansion of the optical flow at the focus of expansion (FOE) is introduced in [5]. When the MAV goes towards the wall-like obstacle directly, the diverging optical flow is generated, as shown in Figure 5, where the optical flows expand from the FOE close to the origin of the image. Using the geometry given in Figure 2, by assuming that FOE and principal point of the image plane coincide and the optical flow expands from the FOE, the following relation can be obtained: where time-to-contact τ can be expressed by the ratio between V x and x. When V x is large or x is small, the time-to-contact becomes small. Using this relation, the time-to-contact τ can be computed. We define the inverse of τ as η, expansion of the optical flow (EOF). Since M r has the same direction with r, the EOF can be computed in a vector form as: Note that the high EOF implies that there is a high risk of the collision with the frontal obstacle. The magnitude of the desired heading rate to avoid the frontal obstacle is determined by the PD controller using the EOF as:ψ where is the difference between the sum of EOF in the left and right half planes. η sum is the sum of the EOF in the horizontal optical flow calculation window and N is the number of pixels in the horizontal optical flow calculation window. k P,η and k D,η are positive gains of the frontal obstacle avoidance controller. Gain values were set to k P,η = 0.8 and k D,η = 0.01 in numerical simulations presented in Section 4, and k P,η = 1.2 and k D,η = 0.01 in the indoor flight experiments presented in Section 5. The sign of e η determines the turning direction, which makes the MAV rotate to the opposite direction in which the obstacle is most likely to be close.

Waypoint Guidance
In addition to the obstacle avoidance, the MAV may need to reach the waypoint to accomplish the given mission. The heading rate and climb rate for waypoint guidance can be determined as: where the error(e ψ ) for the heading rate controller is the angle between the line from the current position to the waypoint and the current heading angle of the MAV ψ c : The error for the climb rate is the difference between the height of the waypoint and the MAV:

Hybrid Obstacle Avoidance Strategy
The hybrid obstacle avoidance strategy is designed to avoid various obstacles in 3-D environment while moving towards the goal according to existence of obstacles determined by the EOF η. The obstacle environment can be classified into three cases: (i) there is no obstacle around the MAV; (ii) there are obstacles but not directly in front of the MAV; and (iii) there are obstacles directly in front of the MAV. According to the classified situation, the obstacle avoidance strategies and waypoint guidance are combined with appropriate weighting factors. The weighting factors are designed with the sigmoid function according to the sum of the EOF: where η eta,0 and η wp,0 represent threshold values and k eta and k wp are positive gains. Figure 6 shows the sigmoid weighting for determining the strategy with k eta = 20, η eta,0 = 3, k wp = 20, and η wp,0 = 2. Sigmoid weights largely contain three sections as the EOF changes. First, in the case that there is no obstacle around the MAV, the weighting for the balance strategy and waypoint guidance are activated and the MAV aims to the waypoint while avoiding the obstacle around the MAV. In the case that there is an obstacle around the MAV, the MAV first need to focus on avoiding the obstacle rather than moving to the waypoint. The MAV will avoid the obstacle not using waypoint guidance but just using the balance strategy. The final situation is that there is an obstacle in front of the MAV where the frontal obstacle avoidance strategy should be used. That is to say, either balance strategy or frontal obstacle avoidance strategy is always activated, and obstacle avoidance strategies are switched depending on the presence or absence of an obstacle directly in front of the MAV. Additionally, waypoint guidance is deactivated if there is a risk of collision and the MAV is required to focus on obstacle avoidance (i.e., η > η wp,0 ). The gains for the sigmoid function were empirically determined. The hybrid obstacle avoidance algorithm including waypoint guidance can then be designed as: Note that, unlike desired heading rate, the desired climb rate can be designed by just adding the vertical balance strategy and waypoint guidance without sigmoid weights. The sigmoid weights should not be multiplied to the vertical balance strategy considering the fact that the sigmoid weights are to change the heading angle of the MAV if there is a wall-like obstacle in front of the MAV.

Simulation Environment Setup
The RotorS simulator [22] was exploited to verify the performance of the obstacle avoidance strategy. The RotorS simulator provides various multi-rotor helicopter models such as the AscTec Hummingbird, Pelican, and Firefly in the Gazebo environment. There are simulated sensors coming with the simulator such as an IMU, a generic odometry sensor, and the VI (Visual-Inertial) sensor, which can be mounted on the multi-rotor helicopters [23]. The RotorS simulator runs on the Robot Operating System (ROS). ROS is a flexible framework for operating various robot software. It is a collection of tools, libraries, and conventions that aim to simplify the task of creating complex and robust robot behavior across a wide variety of robotic platforms [24]. The RotorS simulator receives the desired command using ROS topics. Figure 7 describes the obstacle avoidance simulator structure including RotorS simulator. It uses the desired position P d and the desired heading angle ψ d as guidance and control inputs. Using the desired position, trajectory tracking module generates the throttle command T, the roll angle command φ, and the pitch angle command θ. The attitude controller then generates a moment matrix τ in the body frame. The MAV dynamics makes the MAV move using the throttle and the moment, and provides states that consist of position p, velocity v, orientation q in the quaternion form, and angular velocity w. Using the obstacle avoidance algorithm, the desired position P d and heading angle ψ d are determined as: where P d,x , P d,y , and P d,z are the 3-D desired position; P c,x , P c,y , and P c,z are the current 3-D position; ψ c is the current heading angle; r c is a constant forward distance which can be determined by the forward speed of the MAV; and dt is the sampling frequency.

Simulations Result
Numerical simulations were carried out to verify the performance of the proposed obstacle avoidance strategy. Figure 8 shows the simulation results in L-shape corner, T-shape junction, and ramp-shape maps to verify the horizontal balance, the frontal obstacle avoidance, and the vertical balance strategy, respectively. The first column and second column in Figure 8 show the environment map and the trajectory of the MAV, respectively. For each case, 20 simulations were carried out and the performance of the proposed obstacle avoidance algorithms was analyzed by using the distance to obstacles, as shown in Figure 9. The initial positions were set randomly using the Gaussian distribution with mean and variance set to x ∼ N(0, 1 2 ) and y ∼ N(0, 0.3 2 ) in the L-shape corner and T-shape junction map and x ∼ N(0, 0.3 2 ) and z ∼ N(1.5, 0.3 2 ) in the ramp-shape map. Table 1 shows the success rate, minimum distance to the obstacle and standard deviation of minimum distance averaged for 20 simulations. The success rate was defined by considering the size of the MAV. The size of the AscTec Firefly MAV used in this simulation was 0.605 m, 0.665 m, and 0.165 m for x, y, and z direction, respectively. In the case of the L-shape and T-shape maps, the collision of the MAV was determined if an obstacle was within a certain distance from the center of the MAV; the collision boundary was set to 1.5 times of the half of the y direction size of the MAV. In the case of the ramp-shape map, the MAV was considered to have a collision if the minimum distance was shorter than 1.5 times of the half of z direction size of the MAV. Table 1. Simulation results of obstacle avoidance for three types of map (SD, standard deviation of minimum distance with 20 simulations).  The hybrid obstacle avoidance strategy was also verified in the complex environment surrounded by walls and containing horizontal columns, as described in Figure 10. Figure 11 shows simulation results such as the image from the camera, optical flows and its magnitude at each window, magnitude of the EOF and its time history, and trajectory of the MAV during the simulation. In the EOF time history, the red dashed line represents where the sigmoid weighting for the obstacle avoidance strategy changed rapidly (i.e., η ≥ η eta,0 = 3). At 50 s, as shown in Figure 11a, the EOF was around 2 and the MAV avoided surrounding walls by using the horizontal balance strategy. At 241 s, as shown in Figure 11b, as the MAV approached the frontal wall, the EOF became large and the frontal obstacle avoidance strategy was dominant, which made the MAV turn sharply. At 558 s, as shown in Figure 11c, as the optical flow in the lower half plane in the vertical optical flow window was bigger than in the upper half plane, the MAV avoided the horizontal column by using the vertical balance strategy. The movie clip for the simulation can be found at https://drive.google.com/file/d/1i9Cx2NoRTqqSM99g8u1YIcXqaS8sRJ8k/view.

Yaw Rate Effect Compensation
The obstacle avoidance strategy used in this research assumes that the MAV moves forward without the significant yaw motion. However, if the MAV meets an obstacle, it will turn to avoid the obstacle. If the yaw rate from turning is too large, then the assumption is no longer valid. In other words, a large yaw rate could generate the unintended optical flow and EOF. Especially, when the MAV encounters a frontal obstacle, if it rotates at a large yaw rate, it would make the EOF larger than expected. To verify the effect of yaw rates on the EOF, an illustrative simulation was conducted. Figure 12a shows the trajectories of the MAV moving forward while the turning with the yaw rate of 0, 0.07, 0.14, and 0.21 rad/s. The time history of the EOF for each case is shown in Figure 12b. It shows that the larger the yaw rate is, the larger EOF it generates.
To compensate the effect of the yaw rate on the EOF, the yaw rate compensator was added to the frontal obstacle avoidance strategy (Equation (19)) as: where k ψ is a negative gain andψ is a yaw rate of the MAV. Figure 13a shows the trajectories of the MAV with and without yaw rate compensation. The star symbols on the trajectories are marked every ten seconds. The MAV with the compensator avoided the obstacles smoothly, whereas the MAV without the compensator took an unexpected U-turn. As shown in Figure 13b, since the summation of the EOF exceeded the sigmoid threshold at about 11 s, the obstacle avoidance strategy was changed from the balance strategy to the frontal obstacle avoidance and the yaw rate compensation was applied. In Figure 13c, the red line indicates the yaw rate command of the MAV without yaw rate compensation (ψ η d ). For the MAV with yaw rate compensation, the green and blue lines represent yaw rate compensation command (k ψψ ) and the resultant yaw rate command (ψ η d,comp ), respectively. As the MAV without the compensator approached the obstacles, the EOF and yaw rate increased each other. It resulted in unexpectedly large EOF and yaw rates, as shown in Figure 13d. That is why the MAV without the compensator took the U-turn shown in Figure 13a. On the other hand, the yaw rate compensator alleviated the unexpectedly large yaw rates and EOF as k ψψ took the opposite sign ofψ

Experiment Setup
The hardware for experiments was configured with the customized MAV, as shown in Figure 14. Figure 15 shows the quadrotor MAV setup and indoor localization system (Optitrack) for obstacle avoidance experiments. The NVIDIA Jetson TK1 board was used as the mission computer for obstacle avoidance. It received the image data from the webcam (LifeCam HD-3000, Microsoft, Redmond, WA, USA) and estimated the optical flow using the sequence of the image data. As described in Section 3.5, the obstacle avoidance algorithm required the current pose data to generate a guidance and control command. To estimate the pose at the indoor environment, the motion capture system was exploited. The position data received by the motion capture system were transmitted to the Pixhawk autopilot and fused with sensors such as barometer and magnetometer. The fused data were used to obtain the six-degrees of freedom states of the MAV. By using the computed optical flow and the MAV states, the MAV generated the guidance input expressed by the desired position and heading angle in the Jetson TK1 board and transmitted them to the Pixhawk autopilot. Then, the Pixhawk autopilot drove the motors so that the MAV moved to the desired position and heading angle. The optical flow was calculated with a sequence of the images at 8 Hz and the obstacle avoidance algorithm was run at 1 Hz. Figure 16 shows a sample environment for indoor environments where obstacles were set by brick-patterned boxes so that the optical flow could be generated easily from obstacles.

Experiment Results
To verify obstacle avoidance strategies in a real world, indoor flight experiments were conducted. The waypoint guidance strategy was applied for maintaining the reference height when there was no vertical obstacle around. Figure 17 shows the position of obstacles (green boxes), trajectory of the MAV (black line) and desired position (blue star marks). In the trajectory, the red circles are marked every 1 s. The red circle also means the time that a new control input was generated. In the first environment, the optical flow difference between the left and right half planes was always positive and thus the MAV turned to the right as the MAV went forward. In the second environment, the obstacle was placed in a two-step stair shape. As the optical flow was large at the lower half plane around 1-6 s, as shown in Figure 17d, the MAV increased the height continuously to avoid the obstacle vertically. After passing the obstacle, there was a small difference of the optical flow between upper and lower half planes and the MAV returned to the reference height. In the last environment, the wall-like obstacle existed in front of the MAV. After 5 s, the sum of the EOF increased as the MAV moved towards the wall; as a result, the MAV turned the heading angle abruptly using mainly the frontal obstacle avoidance strategy.

Conclusions and Future Work
This paper proposes the vision-based obstacle avoidance strategy using the optical flow which can be used in various 3-D textured environments. In particular, it exploits the EOF to cope with the front obstacle efficiently along with avoiding obstacles horizontally and vertically. To verify the performance of the proposed approach, numerical simulations and indoor flight experiments were carried out. The proposed obstacle avoidance strategy using optical flows requires a light computational power; hence, it could be readily applicable to miniaturized MAVs requiring the lightweight CPU such as insect-inspired flapping aerial vehicles. It is worth noting that the use of obstacle avoidance based on optical flows is limited to only relatively well-textured environments since optical flows are rarely generated on textureless objects such as white walls, wires and poles.
As the forward speed of the MAV is assumed to be kept relatively slow in our approach, the effect of pitch control on the optical flow computation was negligible. However, for high speed maneuvers, the effect of pitch control on the optical flow would be significant; this will be dealt with in the future work. Besides, although this study proposed the yaw rate effect compensator in the situation where the MAV encounters the frontal obstacle, more rigorous analysis and experiments need to be performed. For general environments, other vision-based algorithms using feature or color detection could be combined with the proposed optical flow-based approach, which remains as future work.