## 5 Three-Dimensional Path Planning Algorithms
### Machine Learning-Based 3D Path Planner
#### 5.1.2 Closed Obstacles  
If the obstacle is closed, our path planner considers the pass-by option with a minimum side clearance from the obstacle. The 3D path planning algorithm takes in bounding boxes information of all detected objects and assigns an identity index to each of them based on the locations of their centers along the y-axis. All information about the bounding box are with respect to the camera frame onboard the UAV. As shown in Figure 5a, the index value increases towards the increasing y-axis of the ZED min stereo camera (in this case, from right to left).  
Figure 4: Open obstacle passing strategy.  
There are three conditions to be considered to determine next setpoint for the UAV. These are searching for  
* Wide primary gaps: gaps between consecutive obstacles;
* Narrow primary gaps but with proximity difference: depth difference between consecutive obstacles; and
* Narrow primary gaps with small or no proximity difference.  
Based on Figure 4(a), the algorithm calculates the primary gap (between the \(i^{th}\) and \((i+1)^{th}\)) and secondary gap (between \(i^{th}\) and \((i+2)^{th}\)). The importance of calculating the secondary gap is that if the primary gap is narrow (less than twice UAV width) but with proximity difference more than twice the UAV length, there is the possibility that the UAV can advance forward but should check whether the secondary gap is wide enough or not to let the UAV pass in between. The gaps and proximity differences are calculated as follows:  
\[\begin{split} Y_{min}^{i+1}-Y_{max}^{i}&\text{ primary gap}\\ Y_{min}^{i+2}-Y_{max}^{i}&\text{secondary gap}\\ \mid X^{i+1}-X^{i}\mid&\text{proximity difference} \end{split} \tag{5}\]  
The pseudo-algorithm of our 3D path planner in the presence of multiple detected obstacles, as shown in Figure 4(b), is given below.  
* index the bounding boxes of the obstacles based on y-axis values of their centers. The box with the smallest y-axis value is indexed as the \(i^{th}\) box;
* calculate \(Y_{min}\) and \(Y_{max}\) for each bounding box;
* calculate the primary gap between the \(i^{th}\) and \((i+1)^{th}\)
* if the gap is greater than or equal to twice UAV width;
* calculate the midpoint of the gap;
* calculate distances from the current location of the UAV to the midpoint and from the midpoint to the goal point. Save the sum of these two distances as path-length;
* else if the primary gap is smaller, calculate the proximity difference of the two consecutive bounding boxes \(i^{th}\) and \((i+1)^{th}\);
* if proximity difference is greater than or equal to twice UAV length, calculate the secondary gap;
* if secondary gap is greater than or equal to twice the UAV width, check the following conditions:
* if the \(i^{th}\) obstacle is closer than the \((i+1)^{th}\), then set \((X^{i},Y_{max}^{i}+obj_{clr},Z^{i})\) as a potential setpoint;
* else, set \((X^{i+2},Y_{min}^{i+2}-obj_{clr},Z^{i+2})\) as a potential setpoint;  
Figure 5: Obstacle-free optimal path selection  
calculate distances from the current location of the UAV to the potential setpoint and from potential setpoint to the goal point. Save the sum of these two distances as path-length;
* apply the above steps for the remaining bounding boxes;
* compare the path-lengths and set the setpoint that leads to a minimum path length as the next setpoint for the UAV;
* else if the secondary gap is less than twice the UAV width, hover at a current altitude and yaw to search for any possible path applying the above procedure;
* if no path is discovered, land the UAV.