## 3 Multi-Domain Informative Path Planning
### Obstacle Avoidance Path Re-Planning
#### 3.2.2 Obstacle Avoidance  
Recall our former assumption of unknown obstacles: we created irregularly shaped obstacles in random locations. The detailed strategy is shown in Algorithm 1. Figure 9: The input of the algorithm is a subpath of the overall path, and the output is the recorded area having strong currents. In the next Section 3.2.2, we discuss the replanning method to avoid the strong currents and obstacles. To prevent collisions, we need to search for free spaces surrounding the obstacle. To replan the whole path to be as short as possible, the replanned path segment should be as close to the former path as possible. In other words, the newfound points should be as near as possible to obstacles but ensure no collisions at the same time. To store the information of obstacles, we use a scapegoat tree to insert them into _terTree_. We update the _treTree_ with the scapegoat tree, or simply reconstruct another KD tree _obTree_ with points on the obstacle's surface. Based on space-partitioning, the KD tree is a data structure that is able to organize points in k-dimensional space. Since the KD tree is balanced, inserting points into it may lead to very deep sorting. However, reconstruction is unnecessary and increases the time cost for the whole algorithm. We solve this problem using the scapegoat tree. The scapegoat tree uses reconstruction criterion \(\alpha\) to ensure the balance of a tree. When a new point is inserted into a tree, trials are conducted to check if any sub-tree contains too many nodes in one side. Our approach to record unknown obstacles is demonstrated in Algorithm 2. We get a flat node from the sub-tree by orderly traversal. ```
Input: Origin Tree of terrain: treTree;  segment path: segpath;  number of spots in segpath: num;  length of vehicle: \(veh_{L}\);  size of corridor for spots in segpath Output: obstacles spots: obstacles;  center of obstacles: obstacle_C;
1 for\(i=1\) to\(num-1\)do
2\(\mathbf{q}\leftarrow\) rand number in \([0,1]\)
3 if\(q>=obstacle\_rate\)then
4\(\mathrm{odist}\leftarrow\) the distance between \(segpath_{i}\) and \(segpath_{i+1}\)
5 calculate the center obstacle_C of obstacle
6 that randomly located between \(segpath_{i}\) and \(segpath_{i+1}\)
7\(1\leftarrow\) random number in \([0,1]\)
8\(obstacle\_{x,y,z}\gets l\cdot(segpath_{i+1}^{x,y,z}-segpath_{i}^{x,y,z})+ segpath_{i}^{x,y,z}\)
9\(j=1\leftarrow\) number of obstacles appear in seg_path;
10 for\(i=1\) to jdo
11\(k=1\)
12 for\(\varphi=0:\overrightarrow{\pi}:2\pi\)do
13 for\(\theta=0:\overrightarrow{\pi}:\pi\)do
14 create obstacles with random size
15 under polar coordinates: \(rand_{\mathrm{rk}}\leftarrow\) random number in \([0,1]\)
16\(obstacle_{x}^{k}=obstacle\_{C}^{i}+veh_{L}\cdot rand_{\mathrm{rk}}\cdot\sin \theta\cos\varphi\)
17\(obstacle_{y}^{k}=obstacle\_{y}^{i}+veh_{L}\cdot rand_{\mathrm{rk}}\cdot\sin \theta\sin\varphi\)
18\(obstacle_{z}^{k}=obstacle\_{z}^{i}+veh_{L}\cdot rand_{\mathrm{rk}}\cdot\cos\theta\)
19
20 return Obstacle, Obstacle_C;
```  
**Algorithm 1**Generate the randomly unknown obstacles. For obstacle \(obs_{s}\) in the path segment between \(p_{m+i}\) and \(p_{m+i+1}\), a circumscribed sphere \(SPH_{s}^{1}\) with the farthest distance \(r_{s}^{1}\) from \([obs_{s}^{1},obs_{s}^{2},....obs_{s}^{k}]\) as the radius can be drawn out of \(SPH_{s}^{1}\), which is free of obstacles. Let \(SPH_{s}^{2}\) be the extension of \(SPH_{s}^{1}\), whose radius is denoted as \(r_{s}^{2}=veh_{L}+r_{s}^{1}\). Spots on \(SPH_{s}^{2}\) are the nearest spots to obstacles that can ensure safe flight for the HAUV. With the purpose of generating a smooth, safety trajectory, we tend to form corridors to record free space, which are used as space constraints (as shown in the next section). Here we focus on the approach to calculate specific size parameters of corridors. A trajectory can be generated only if all corridors coincide. With unknown environmental information recorded, let \(D_{obs}^{j}\) be the shortest distance between obstacle \(obs_{s}\) and \(SPH_{s}^{2}\).