## 3 Multi-Domain Informative Path Planning
### Obstacle Avoidance Path Re-Planning
#### 3.2.2 Obstacle Avoidance
\(D_{mi}\) is denoted as the minimum distance between \(p_{m+i}(x_{m+i},y_{m+i},z_{m+i})\) and \(obs_{s}\). The side length \(L_{mi}\) of each cube corridor is defined as follows:  
\[L_{mi}=\sqrt{2}*D_{mi} \tag{10}\]  
As illustrated in Figure 10, a sphere with a radius equal to \(D_{mi}\) is circumscribed to mark free space. ```
Input: spots to be inserted: obstacle; The origin tree: treTree; control parameter: \(\alpha\) Output: newly constructed tree: treTree
1N \(\leftarrow\) number of obstacle spots for\(i=1\)to\(N\)do
2 Tree \(\leftarrow\) insert \(obstacle_{i}\) into Tree for\(node_{i}\) in \(n\)Treedo
3 leftset \(\leftarrow\) number of elements in \(node_{i}\)'s left subtree
4 rightset \(\leftarrow\) number of elements in \(node_{i}\)'s left right subtree
5 Allset \(\leftarrow\) leftset \(+\) rightset
6 if\(leftset/Allset\geq\alpha\) or \(<1-\alpha\)then
7 flat data \(\leftarrow\) ergodic subtree of \(node_{i}\)
8
9\(\leftarrow\)\(\leftarrow\) KD_constructed (flat data)
10 Tree \(\leftarrow\) use nodeTree to replace \(node_{i}\)'s subtree in Tree
11
12 return
```  
**Algorithm 2**Scepegoat_insert. We use a square instead of a circle to record the free space, because it can easily set constraints at each axis out of \(x,y,z\) in Cartesian coordinates, as is shown in Figure 11.  
Therefore, the trajectory between \(p_{m+i}\) and \(p_{m+i+1}\) exists only when:  
Figure 10: Generating the corridor size for target spots found on \(SPH_{s}^{2}\). If the corridor intersects with the \(p_{m+i}^{\prime}\)s and \(p_{m+i+1}^{\prime}\)s corridor, then the spots will be recorded in the target spots set. \[ \begin{split}\frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi}}{\sqrt{2}}& \geq|x_{ob}-x_{m+i}|\\ \frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi}}{\sqrt{2}}& \geq|y_{ob}-y_{m+i}|\\ \frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi}}{\sqrt{2}}& \geq|z_{ob}-z_{m+i}|\\ \frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi+1}}{\sqrt{2}}& \geq|x_{ob}-x_{m+i+1}|\\ \frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi+1}}{\sqrt{2}}& \geq|y_{ob}-y_{m+i+1}|\\ \frac{D_{obs}^{j}}{\sqrt{2}}+\frac{D_{mi+1}}{\sqrt{2}}& \geq|z_{ob}-z_{m+i+1}|\\ \end{split}  \tag{11}\]  
The points that satisfy the geometric constraints are recorded in expected points set _obset_. Given a large enough graph map, each obstacle \(obs_{s}\) should have at least one such set of points \(obset_{s}\). The method of searching available points on a collision-free sphere is demonstrated in Figure 12.  
Figure 11: Two-dimensional corridors of \(p_{new}\) and \(p^{\prime}_{m+i}s\) and \(p^{\prime}_{m+i+1}s\). In this condition, if the corridor of \(p_{new}\) intersects with the \(p^{\prime}_{m+i}s\) and \(p^{\prime}_{m+i+1}s\) corridor, then the spots will be recorded as part of the target spots set. Figure 12: Algorithmic framework of subset (the set of collision-free spots on \(SPH_{6}^{2,3,\sim}\)) searching. With the coordinates and corridor sizes of spots in the original path segment, the program will run until spots expected on \(SPH_{6}^{2,3,\sim}\) are found. Let the expected spots be an object. If no expected points on \(SPH_{s}^{2}\) are found, the radius \(R\) will be extended by step size \(r\), and searching will keep going until at least one expected point is recorded. The flow chart of the whole obstacle avoidance method is shown in Figure 12(a) (sacepegoat tree) and Figure 12(b) (reconstructing trees). The inputs of the scapegoat tree are the coordinates of obstacle spots, the centers of the obstacles, the path segment, and the original _treTree_. After all obstacle spots are inserted into _treTree_, the radius of the collision-free sphere will be updated until expected spots are found. Obstet contains all the expected spots and is the output of this part of the program. Details are shown in Algorithm 3. On the other hand, the replanning method constructs a new KD tree from obstacle spots, as shown in Algorithm 4. Figure 12: Flow Chart For obstacle avoidance, both the scapegoat tree and KD tree reconstruction methods are illustrated.